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Preface 


In the last two decades, there has been a tremendous surge of activity 
in robotics, both at in terms of research and in terms of capturing the 
imagination of the general public as to its seemingly endless and diverse 
possibilities. This period has been accompanied by a technological mat¬ 
uration of robots as well, from the simple pick and place and painting 
and welding robots, to more sophisticated assembly robots for inserting 
integrated circuit chips onto printed circuit boards, to mobile carts for 
parts handling and delivery. Several areas of robotic automation have 
now become “standard” on the factory floor and, as of the writing of 
this book, the field is on the verge of a new explosion to areas of growth 
involving hazardous environments, minimally invasive surgery, and micro 
electro-mechanical mechanisms. 

Concurrent with the growth in robotics in the last two decades has 
been the development of courses at most major research universities on 
various aspects of robotics. These courses are taught at both the under¬ 
graduate and graduate levels in computer science, electrical and mechan¬ 
ical engineering, and mathematics departments, with different emphases 
depending on the background of the students. A number of excellent 
textbooks have grown out of these courses, covering various topics in 
kinematics, dynamics, control, sensing, and planning for robot manipu¬ 
lators. 

Given the state of maturity of the subject and the vast diversity of stu¬ 
dents who study this material, we felt the need for a book which presents 
a slightly more abstract (mathematical) formulation of the kinematics, 
dynamics, and control of robot manipulators. The current book is an 
attempt to provide this formulation not just for a single robot but also 
for multifingered robot hands, involving multiple cooperating robots. It 
grew from our efforts to teach a course to a hybrid audience of electrical 
engineers who did not know much about mechanisms, computer scientists 
who did not know about control theory, mechanical engineers who were 
suspicious of involved explanations of the kinematics and dynamics of 
garden variety open kinematic chains, and mathematicians who were cu¬ 
rious, but did not have the time to build up lengthy prerequisites before 


beginning a study of robotics. 

It is our premise that abstraction saves time in the long run, in return 
for an initial investment of effort and patience in learning some mathe¬ 
matics. The selection of topics—from kinematics and dynamics of single 
robots, to grasping and manipulation of objects by multifingered robot 
hands, to nonholonomic motion planning—represents an evolution from 
the more basic concepts to the frontiers of the research in the field. It 
represents what we have used in several versions of the course which 
have been taught between 1990 and 1993 at the University of California, 
Berkeley, the Courant Institute of Mathematical Sciences of New York 
University, the California Institute of Technology, and the Hong Kong 
University of Science and Technology (HKUST). We have also presented 
parts of this material in short courses at the Universita di Roma, the 
Center for Artificial Intelligence and Robotics, Bangalore, India, and the 
National Taiwan University, Taipei, Taiwan. 

The material collected here is suitable for advanced courses in robotics 
consisting of seniors or first- and second-year graduate students. At a 
senior level, we cover Chapters 1-4 in a twelve week period, augmenting 
the course with some discussion of technological and planning issues, as 
well as a laboratory. The laboratory consists of experiments involving 
on-line path planning and control of a few industrial robots, and the 
use of a simulation environment for off-line programming of robots. In 
courses stressing kinematic issues, we often replace material from Chapter 
4 (Robot Dynamics) with selected topics from Chapter 5 (Multifingered 
Hand Kinematics). We have also covered Chapters 5-8 in a ten week 
period at the graduate level, in a course augmented with other advanced 
topics in manipulation or mobile robots. 

The prerequisites that we assume are a good course in linear algebra 
at the undergraduate level and some familiarity with signals and systems. 
A course on control at the undergraduate level is helpful, but not strictly 
necessary for following the material. Some amount of mathematical ma¬ 
turity is also desirable, although the student who can master the concepts 
in Chapter 2 should have no difficulty with the remainder of the book. 

We have provided a fair number of exercises after Chapters 2-8 to help 
students understand some new material and review their understanding of 
the chapter. A toolkit of programs written in Mathematica for solving the 
problems of Chapters 2 and 3 (and to some extent Chapter 5) have been 
developed and are described in Appendix B. We have studiously avoided 
numerical exercises in this book: when we have taught the course, we 
have adapted numerical exercises from measurements of robots or other 
“real” systems available in the laboratories. These vary from one time to 
the next and add an element of topicality to the course. 

The one large topic in robotic manipulation that we have not covered 
in this book is the question of motion planning and collision avoidance 


xiv 


for robots. In our classroom presentations we have always covered some 
aspects of motion planning for robots for the sake of completeness. For 
graduate classes, we can recommend the recent book of Latombe on mo¬ 
tion planning as a supplement in this regard. Another omission from this 
book is sensing for robotics. In order to do justice to this material in our 
respective schools, we have always had computer vision, tactile sensing, 
and other related topics, such as signal processing, covered in separate 
courses. 

The contents of our book have been chosen from the point of view 
that they will remain foundational over the next several years in the face 
of many new technological innovations and new vistas in robotics. We 
have tried to give a snapshot of some of these vistas in Chapter 9. In 
reading this book, we hope that the reader will feel the same excitement 
that we do about the technological and social prospects for the field of 
robotics and the elegance of the underlying theory. 

Richard Murray Berkeley, August 1993 

Zexiang Li 
Shankar Sastry 
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Chapter 1 

Introduction 


In the last twenty years, our conception and use of robots has evolved 
from the stuff of science fiction films to the reality of computer-controlled 
electromechanical devices integrated into a wide variety of industrial en¬ 
vironments. It is routine to see robot manipulators being used for welding 
and painting car bodies on assembly lines, stuffing printed circuit boards 
with IC components, inspecting and repairing structures in nuclear, un¬ 
dersea, and underground environments, and even picking oranges and 
harvesting grapes in agriculture. Although few of these manipulators 
are anthropomorphic, our fascination with humanoid machines has not 
dulled, and people still envision robots as evolving into electromechanical 
replicas of ourselves. While we are not likely to see this type of robot in 
the near future, it is fair to say that we have made a great deal of progress 
in introducing simple robots with crude end-effectors into a wide variety 
of circumstances. Further, it is important to recognize that our impa¬ 
tience with the pace of robotics research and our expectations of what 
robots can and cannot do is in large part due to our lack of appreciation 
of the incredible power and subtlety of our own biological motor control 
systems. 


1 Brief History 

The word robot was introduced in 1921 by the Czech playwright Karel 
Capek in his satirical play R. U. R. (Rossum’s Universal Robots), where 
he depicted robots as machines which resembled people but worked tire¬ 
lessly. In the play, the robots eventually turn against their creators and 
annihilate the human race. This play spawned a great deal of further sci¬ 
ence fiction literature and film which have contributed to our perceptions 
of robots as being human-like, endowed with intelligence and even per¬ 
sonality. Thus, it is no surprise that present-day robots appear primitive 
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Figure 1.1: The Stanford manipulator. (Courtesy of the Coordinated 
Science Laboratory, University of Illinois at Urbana-Champaign) 


when compared with the expectations created by the entertainment in¬ 
dustry. To give the reader a flavor of the development of modern robotics, 
we will give a much abbreviated history of the field, derived from the ac¬ 
counts by Fu, Gonzalez, and Lee [35] and Spong and Vidyasagar [110]. 
We describe this roughly by decade, starting from the fifties and contin¬ 
uing up to the eighties. 

The early work leading up to today’s robots began after World War 
II in the development of remotely controlled mechanical manipulators, 
developed at Argonne and Oak Ridge National Laboratories for handling 
radioactive material. These early mechanisms were of the master-slave 
type, consisting of a master manipulator guided by the user through a 
series of moves which were then duplicated by the slave unit. The slave 
unit was coupled to the master through a series of mechanical linkages. 
These linkages were eventually replaced by either electric or hydraulic 
powered coupling in “teleoperators,” as these machines are called, made 
by General Electric and General Mills. Force feedback to keep the slave 
manipulator from crushing glass containers was also added to the teleop¬ 
erators in 1949. 

In parallel with the development of the teleoperators was the devel- 
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Figure 1.2: The Cincinnati Milacron T 3 (The Tomorrow Tool) robot. 
(Courtesy of Cincinnati Milacron) 


opment of Computer Numerically Controlled (CNC) machine tools for 
accurate milling of low-volume, high-performance aircraft parts. The 
first robots, developed by George Devol in 1954, replaced the master 
manipulator of the teleoperator with the programmability of the CNC 
machine tool controller. He called the device a “programmed articulated 
transfer device.” The patent rights were bought by a Columbia Univer¬ 
sity student, Joseph Engelberger, who later founded a company called 
Unimation in Connecticut in 1956. Unimation installed its first robot in 
a General Motors plant in 1961. The key innovation here was the “pro¬ 
grammability” of the machine: it could be retooled and reprogrammed 
at relatively low cost so as to enable it to perform a wide variety of 
tasks. The mechanical construction of the Unimation robot arm repre¬ 
sented a departure from conventional machine design in that it used an 
open kinematic chain: that is to say, it had a cantilevered beam structure 
with many degrees of freedom. This enabled the robot to access a large 
workspace relative to the space occupied by the robot itself, but it cre¬ 
ated a number of problems for the design since it is difficult to accurately 
control the end point of a cantilevered arm and also to regulate its stiff¬ 
ness. Moreover, errors at the base of the kinematic chain tended to get 
amplified further out in the chain. To alleviate this problem, hydraulic 
actuators capable of both high power and generally high precision were 
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Figure 1.3: The Unimation PUMA (Programmable Universal Manipula¬ 
tor for Assembly). (Courtesy of Staubli Unimation, Inc.) 


used for the joint actuators. 

The flexibility of the newly introduced robots was quickly seen to be 
enhanced through sensory feedback. To this end, Ernst in 1962 devel¬ 
oped a robot with force sensing which enabled it to stack blocks. To our 
knowledge, this system was the first that involved a robot interacting 
with an unstructured environment and led to the creation of the Project 
MAC (Man And Computer) at MIT. Tomovic and Boni developed a pres¬ 
sure sensor for the robot which enabled it to squeeze on a grasped object 
and then develop one of two different grasp patterns. At about the same 
time, a binary robot vision system which enabled the robot to respond to 
obstacles in its environment was developed by McCarthy and colleagues 
in 1963. Many other kinematic models for robot arms, such as the Stan¬ 
ford manipulator, the Boston arm, the AMF (American Machine and 
Foundry) arm, and the Edinburgh arm, were also introduced around this 
time. Another novel robot of the period was a walking robot developed 
by General Electric for the Army in 1969. Robots that responded to 
voice commands and stacked randomly scattered blocks were developed 
at Stanford and other places. Robots made their appearance in Japan 
through Kawasaki’s acquisition of a license from Unimation in 1968. 
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Figure 1.4: The AcleptOne robot. (Courtesy of Adept Technology, Inc.) 



Figure 1.5: The CMU DD Arm I. (Courtesy of M.J. Dowling) 
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Figure 1.6: The Odex I six-legged walking robot. (Photo courtesy of 
Odetics, Inc.) 


In 1973, the first language for programming robot motion, called 
WAVE, was developed at Stanford to enable commanding a robot with 
high-level commands. About the same time, in 1974, the machine tool 
manufacturer Cincinnati Milacron, Inc. introduced its first computer- 
controlled manipulator, called The Tomorrow Tool (T 3 ), which could lift 
a 100 pound load as well as track moving objects on an assembly line. 
Later in the seventies, Paul and Bolles showed how a Stanford arm could 
assemble water pumps, and Will and Grossman endowed a robot with 
touch and force sensors to assemble a twenty part typewriter. At roughly 
the same time, a group at the Draper Laboratories put together a Remote 
Center Compliance (RCC) device for part insertion in assembly. 

In 1978, Unimation introduced a robot named the Programmable Uni¬ 
versal Machine for Assembly (PUMA), based on a General Motors study. 
Bejczy at Jet Propulsion Laboratory began a program of teleoperation 
for space-based manipulators in the mid-seventies. In 1979, the SCARA 
(Selective Compliant Articulated Robot for Assembly) was introduced in 
Japan and then in the United States. 

As applications of industrial robots grew, different kinds of robots 
with attendant differences in their actuation methods were developed. 
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For light-duty applications, electrically powered robots were used both 
for reasons of relative inexpensiveness and cleanliness. The difficulty with 
electric motors is that they produce their maximum power at relatively 
high speeds. Consequently, the motors need to be geared down for use. 
This gear reduction introduces friction, backlash, and expense to the de¬ 
sign of the motors. Consequently, the search was on to find a way of 
driving the robot’s joints directly without the need to gear down its elec¬ 
tric motors. In response to this need, a direct drive robot was developed 
at Carnegie Mellon by Asada in 1981. 

In the 1980s, many efforts were made to improve the performance 
of industrial robots in fine manipulation tasks: active methods using 
feedback control to improve positioning accuracy and program compli¬ 
ance, and passive methods involving a mechanical redesign of the arm. 
It is fair to say, however, that the eighties were not a period of great 
innovation in terms of building new types of robots. The major part of 
the research was dedicated to an understanding of algorithms for con¬ 
trol, trajectory planning, and sensor aggregation of robots. Among the 
first active control methods developed were efficient recursive Lagrangian 
and computational schemes for computing the gravity and Coriolis force 
terms in the dynamics of robots. In parallel with this, there was an effort 
in exactly linearizing the dynamics of a multi-joint robot by state feed¬ 
back, using a technique referred to as computed torque. This approach, 
while computationally demanding, had the advantage of giving precise 
bounds on the transient performance of the manipulator. It involved ex¬ 
act cancellation, which in practice had to be done either approximately or 
adaptively. There were may other projects on developing position/force 
control strategies for robots in contact with the environment, referred to 
as hybrid or compliant control. In the search for more accurately control¬ 
lable robot manipulators, robot links were getting to be lighter and to 
have harmonic drives, rather than gear trains in their joints. This made 
for flexible joints and arms, which in turn necessitated the development 
of new control algorithms for flexible link and flexible joint robots. 

The trend in the nineties has been towards robots that are modifiable 
for different assembly operations. One such robot is called Robotworld, 
manufactured by Automatix, which features several four degree of free¬ 
dom modules suspended on air bearings from the stator of a Sawyer 
effect motor. By attaching different end-effectors to the ends of the mod¬ 
ules, the modules can be modified for the assembly task at hand. In 
the context of robots working in hazardous environments, great strides 
have been made in the development of mobile robots for planetary ex¬ 
ploration, hazardous waste disposal, and agriculture. In addition to the 
extensive programs in reconfigurable robots and robots for hazardous en¬ 
vironments, we feel that the field of robotics is primed today for some 
large technological advances incorporating developments in sensor and 
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actuator technology at the milli- and micro-scales as well as advances 
in computing and control. We defer a discussion of these prospects for 
robotics to Chapter 9. 

2 Multifingered Hands and Dextrous Ma¬ 
nipulation 

The vast majority of robots in operation today consist of six joints which 
are either rotary (articulated) or sliding (prismatic), with a simple “end- 
effector” for interacting with the workpieces. The applications range from 
pick and place operations, to moving cameras and other inspection equip¬ 
ment, to performing delicate assembly tasks involving mating parts. This 
is certainly nowhere near as fancy as the stuff of early science fiction, but 
is useful in such diverse arenas such as welding, painting, transportation 
of materials, assembly of printed circuit boards, and repair and inspection 
in hazardous environments. 

The term hand or end- effector is used to describe the interface between 
the manipulator (arm) and the environment, out of anthropomorphic 
intent. The vast majority of hands are simple: grippers (either two- or 
three-jaw), pincers, tongs, or in some cases remote compliance devices. 
Most of these end-effectors are designed on an ad hoc basis to perform 
specific tasks with specific parts. For example, they may have suction 
cups for lifting glass which are not suitable for machined parts, or jaws 
operated by compressed air for holding metallic parts but not suitable 
for handling fragile plastic parts. Further, a difficulty that is commonly 
encountered in applications is the clumsiness of a six degree of freedom 
robot equipped only with these simple hands. The clumsiness manifests 
itself in: 

1. A lack of dexterity. Simple grippers enable the robot to hold parts 
securely but they cannot manipulate the grasped object. 

2. A limited number of possible grasps resulting in the need to change 
end-effectors frequently for different tasks. 

3. Large motions of the arm are sometimes needed for even small mo¬ 
tions of the end-effector. Since the motors of the robot arm are 
progressively larger away from the end-effector, the motion of the 
earliest motors is slow and inefficient. 

4. A lack of fine force control which limits assembly tasks to the most 
rudimentary ones. 

A multifingered or articulated hand offers some solutions to the prob¬ 
lem of endowing a robot with dexterity and versatility. The ability of a 



Medial Lateral 

Figure 1.7: Homunculus diagram of the motor cortex. (Reprinted, by 
permission, from Kandel, Schwartz, and Jessel, Principles of Neural Sci¬ 
ence , Third Edition [Appleton and Lange, Norwalk, CT, 1991]. Adapted 
from Penfield and Rasmussen, The Cerebral Cortex of Man: A Clinical 
Study of Localization of Function [Macmillan, 1950]) 


multifingered hand to reconfigure itself for performing a variety of differ¬ 
ent grasps reduces the need for changing end-effectors. The large number 
of lightweight actuators associated with the degrees of freedom of the 
hand allows for fast, precise, and energy-efficient motions of the object 
held in the hand. Fine motion force-control at a high bandwidth is also 
facilitated for similar reasons. Indeed, multifingered hands are a truly 
anthropomorphically motivated concept for dextrous manipulation: we 
use our arms to position our hands in a given region of space and then 
use our wrists and fingers to interact in a detailed and intricate way with 
the environment. We preform our fingers into grasps which pinch, en¬ 
circle, or immobilize objects, changing grasps as a consequence of these 
actions. One measure of the intelligence of a member of the mammalian 
family is the fraction of its motor cortex dedicated to the control of its 
hands. This fraction is discerned by painstaking mapping of the body 
on the motor cortex by neurophysiologists, yielding a homunculus of the 
kind shown in Figure 1.7. For humans, the largest fraction (30-40%) of 
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Figure 1.8: The Utah/MIT hand. (Photo courtesy of Sarcos, Inc.) 


the motor cortex is dedicated to the control of the hands, as compared 
with 20-30% for most monkeys and under 10% for dogs and cats. 

From a historical point of view, the first uses of multifingered hands 
were in prosthetic devices to replace lost limbs. Childress [18] refers to 
a device from 1509 made for a knight, von Berlichingen, who had lost 
his hand in battle at an early age. This spring-loaded device was useful 
in battle but was unfortunately not handy enough for everyday func¬ 
tions. After the Berlichingen hand, numerous other hand designs have 
been made from 1509 to the current time. Several of these hands are 
still available today; some are passive (using springs), others are body- 
powered (using arm flexion control or shrug control). Some of the hands 
had the facility for voluntary closure and others involuntary closure. Chil¬ 
dress classifies the hands into the following four types: 

1. Cosmetic. These hands have no real movement and cannot be acti¬ 
vated, but they can be used for pushing or as an opposition element 
for the other hand. 

2. Passive. These hands need the manual manipulation of the other 
(non-prosthetic) hand to adjust the grasping of the hand. These 
were the earliest hands, including the Berlichingen hand. 


10 





Figure 1.9: The Salisbury Hand, designed by Kenneth Salisbury. (Photo 
courtesy of David Lampe, MIT) 


3. Body powered. These hands use motions of the body to activate the 
hand. Two of the most common schemes involve pulling a cable 
when the arm is moved forward (arm-flexion control) or pulling 
the cable when the shoulders are rounded (shrug control). Indeed, 
one frequently observes these hands operated by an amputee using 
shrugs and other such motions of her upper arm joints. 

4. Externally powered. These hands obtain their energy from a stor¬ 
age source such as a battery or compressed gas. These are yet to 
displace the body-powered hands in prostheses. 

Powered hand mechanisms came into vogue after 1920, but the great¬ 
est usage of these devices has been only since the 1960s. The Belgrade 
hand, developed by Tomovic and Boni [113], was originally developed for 
Yugoslav veterans who had lost their arms to typhus. Other hands were 
invented as limb replacements for “thalidomide babies.” There has been 
a succession of myoelectrically controlled devices for prostheses culminat¬ 
ing in some advanced devices at the University of Utah [44], developed 
mainly for grasping objects. While these devices are quite remarkable 
mechanisms, it is fair to say that their dexterity arises from the vision- 
guided feedback training of the wearer, rather than any feedback mecha¬ 
nisms inherent in the device per se. 

As in the historical evolution of robots, teleoperation in hazardous or 
hard to access environments—such as nuclear, underwater, space, mining, 
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Figure 1.10: Styx, a two-fingered planar hand built at UC Berkeley in 
1988. 


and, recently, surgical environments—has provided a large impetus for 
the development of dextrous multifingered hands. These devices enable 
the operator to perform simple manipulations with her hands in a remote 
environment and have the commands be relayed to a remote multifingered 
manipulator. In the instance of surgery, the remote manipulator is a 
surgical device located inside the body of the patient. 

There have been many attempts to devise multifingered hands for 
research use which are somewhere between teleoperation, prosthesis, and 
dextrous end-effectors. These hands truly represent our dual point of 
view in terms of jumping back and forth from an anthropomorphic point 
of view (mimicking our own hands) to the point of view of intelligent 
end-effectors (for endowing our robots with greater dexterity). Some 
examples of research on multifingered hands can be found in the work 
of Skinner [106], Okada [84], and Hanafusa and Asada [39]. The Okada 
hand was a three-fingered cable-driven hand which accomplished tasks 
such as attaching a nut to a bolt. Hanafusa and Asada’s hand has three 
elastic fingers driven by a single motor with three claws for stably grasping 
several oddly shaped objects. 

Later multifingered hands include the Salisbury Hand (also known 
as the Stanford/JPL hand) [69], the Utah/MIT hand [44], the NYU 
hand [24], and the research hand Styx [76]. The Salisbury hand is a 
three-fingered hand; each finger has three degrees of freedom and the 
joints are all cable driven. The placement of the fingers consists of one 
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finger (the thumb) opposing the other two. The Utah/MIT hand has 
four fingers (three fingers and a thumb) in a very anthropomorphic con¬ 
figuration; each finger has four degrees of freedom and the hand is cable 
driven. The difference in actuation between the Salisbury Hand and the 
Utah/MIT hand is in how the cables (tendons) are driven: the first uses 
electric motors and the second pneumatic pistons. The NYU hand is a 
non-anthropomorphic planar hand with four fingers moving in a plane, 
driven by stepper motors. Styx was a two-fingered hand with each finger 
having two joints, all direct driven. Like the NYU hand, Styx was used 
as a test bed for performing control experiments on multifingered hands. 

At the current time, several kinds of multifingered hands at differ¬ 
ent scales—down to millimeters and even micrometers—are either being 
developed or put in use. Some of them are classified merely as custom 
or semi-custom end-effectors. A recent multifingered hand developed in 
Pisa is used for picking oranges in Sicily, another developed in Japan is 
used to play a piano! One of the key stumbling blocks to the development 
of lightweight hands has been lightweight high-torque motors. In this re¬ 
gard, muscle-like actuators, inch-worm motors, and other novel actuator 
technologies have been proposed and are currently being investigated. 
One future application of multifmgered robot hands which relies on these 
technologies is in minimally invasive surgery. This application is further 
discussed in Chapter 9. 


3 Outline of the Book 

This book is organized into eight chapters in addition to this one. Most 
chapters contain a summary section followed by a set of exercises. We 
have deliberately not included numerical exercises in this book. In teach¬ 
ing this material, we have chosen numbers for our exercises based on 
some robot or other physical situation in the laboratory. We feel this 
adds greater realism to the numbers. 

Chapter 2 is an introduction to rigid body motion. In this chapter, we 
present a geometric view to understanding translational and rotational 
motion of a rigid body. While this is one of the most ubiquitous topics 
encountered in textbooks on mechanics and robotics, it is also perhaps 
one of the most frequently misunderstood. The simple fact is that the 
careful description and understanding of rigid body motion is subtle. The 
point of view in this chapter is classical, but the mathematics modern. 
After defining rigid body rotation, we introduce the use of the expo¬ 
nential map to represent and coordinatize rotations (Euler’s theorem), 
and then generalize to general rigid motions. In so doing, we introduce 
the notion of screws and twists, and describe their relationship with ho¬ 
mogeneous transformations. With this background, we begin the study 
of infinitesimal rigid motions and introduce twists for representing rigid 
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body velocities. The dual of the theory of twists is covered in a section 
on wrenches, which represent generalized forces. The chapter concludes 
with a discussion of reciprocal screws. In classroom teaching, we have 
found it important to cover the material of Chapter 2 at a leisurely pace 
to let students get a feel for the subtlety of understanding rigid body 
motion. 

The theory of screws has been around since the turn of the century, 
and Chasles’ theorem and its dual, Poinsot’s theorem, are even more 
classical. However, the treatment of the material in this chapter eas¬ 
ily extends to other more abstract formulations which are also useful 
in thinking about problems of manipulation. These are covered in Ap¬ 
pendix A. 

The rest of the material in the book may be subdivided into three 
parts: an introduction to manipulation for single robots, coordinated ma¬ 
nipulation using a multifingered robot hand, and nonholonomic motion 
planning. We will discuss the outline of each part in some detail. 

3.1 Manipulation using single robots 

Chapter 3 contains the description of manipulator kinematics for a single 
robot. This is the description of the position and orientation of the end- 
effector or gripper in terms of the angles of the joints of the robot. The 
form of the manipulator kinematics is a natural outgrowth of the exponen¬ 
tial coordinatization for rigid body motion of Chapter 2. We prove that 
the kinematics of open-link manipulators can be represented as a product 
of exponentials. This formalism, first pointed out by Brockett [12], is el¬ 
egant and combines within it a great deal of the analytical sophistication 
of Chapter 2. Our treatment of kinematics is something of a deviation 
from most other textbooks, which prefer a Denavit-Hartenberg formula¬ 
tion of kinematics. The payoff for the product of exponentials formalism 
is shown in this chapter in the context of an elegant formulation of a 
set of canonical problems for solving the inverse kinematics problem: the 
problem of determining the joint angles given the position and orienta¬ 
tion of the end-effector or gripper of the robot. These problems, first 
formulated by Paden and Kahan [85], enable a precise determination of 
all of the multiple inverse kinematic solutions for a large number of indus¬ 
trial robots. The extension of this approach to the inverse kinematics of 
more general robots actually needs some recent techniques from classical 
algebraic geometry, which we discuss briefly. 

Another payoff of using the product of exponentials formula for kine¬ 
matics is the ease of differentiating the kinematics to obtain the manipu¬ 
lator Jacobian. The columns of the manipulator Jacobian have the inter¬ 
pretation of being the twist axes of the manipulator. As a consequence, it 
is easy to geometrically characterize and describe the singularities of the 
manipulator. The product of exponentials formula is also used for deriv- 
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ing the kinematics of robots with one or more closed kinematic chains, 
such as a Stewart platform or a four-bar planar linkage. 

Chapter 4 is a derivation of the dynamics and control of single robots. 

We start with a review of the Lagrangian equations of motion for a system 
of rigid bodies. We also specialize these equations to derive the Newton- 
Euler equations of motion of a rigid body. As in Chapter 2, this material 
is classical but is covered in a modern mathematical geometric frame¬ 
work. Using once again the product of exponentials formula, we derive 
the Lagrangian of an open-chain manipulator and show how the geomet¬ 
ric structure of the kinematics reflects into the form of the Lagrangian of 
the manipulator. 

Finally, we review the basics of Lyapunov theory to provide some 
machinery for proving the stability of the control schemes that we will 
present in this book. We use this to prove the stability of two classes 
of control laws for single manipulators: the computed torque control law 
and the so-called PD (for proportional + derivative) control law for single 
manipulators. 

3.2 Coordinated manipulation using multifingered robot 
hands 

Chapter 5 is an introduction to the kinematics of grasping. Beginning 
with a review of models of contact types, we introduce the notion of a 
grasp map, which expresses the relationship between the forces applied by 
the fingers contacting the object and their effect at the center of mass of 
the object. We characterize what are referred to as stable grasps or force- 
closure grasps. These are grasps which immobilize an object robustly. 
Using this characterization, we discuss how to construct force-closure 
grasps using an appropriate positioning of the fingers on the surface of 
the object. 

The first half of the chapter deals with issues of force exerted on the 
object by the fingers. The second half deals with the dual issue of how 
the movements of the grasped object reflect the movements of the fingers. 

This involves the interplay between the qualities of the grasp and the 
kinematics of the fingers (which are robots in their own right) grasping the 
object. A definition dual to that of force-closure, called manipulability, 
is defined and characterized. Finally, we discuss the rolling of fingertips 
on the surface of an object. This is an important way of repositioning 
fingers on the surface of an object so as to improve a grasp and may be 
necessitated by the task to be performed using the multifingered hand. 

Chapter 6 is a derivation of the dynamics and control for multifingered 
robot hands. The derivation of the kinematic equations for a multifin¬ 
gered hand is an exercise in writing equations for robotic systems with 
constraints, namely the constraints imposed by the grasp. We develop the 
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necessary mathematical machinery for writing the Lagrangian equations 
for systems with so-called Pfaffian constraints. There is a preliminary dis¬ 
cussion of why these Pfaffian or velocity constraints cannot be simplified 
to constraints on the configuration variables of the system alone. Indeed, 
this is the topic of Chapters 7 and 8. We use our formalism to write the 
equations of motion for a multifingered hand system. We show connec¬ 
tions between the form of these equations and the dynamical equations 
for a single robot. The dynamical equations are particularly simple when 
the grasps are nonredundant and manipulable. In the instance that the 
grasps are either redundant or nonmanipulable, some substantial changes 
need to be made to their dynamics. Using the form of dynamical equa¬ 
tions for the multifingered hand system, we propose two separate sets of 
control laws which are reminiscent of those of the single robot, namely 
the computed torque control law and the PD control law, and prove their 
performance. 

A large number of multifingered hands, including those involved in the 
study of our own musculo-skeletal system, are driven not by motors but 
by networks of tendons. In this case, the equations of motion need to be 
modified to take into account this mechanism of force generation at the 
joints of the fingers. This chapter develops the dynamics of tendon-driven 
robot hands. 

Another important topic to be considered in the control of systems 
of many degrees of freedom, such as the multifingered robot hand, is the 
question of the hierarchical organization of the control. The computed 
torque and PD control law both suffer from the drawback of being com¬ 
putationally expensive. One could conceive that a system with hundreds 
of degrees of freedom, such as the mammalian musculo-skeletal system, 
has a hierarchical organization with coarse control at the cortical level 
and progressively finer control at the spinal and muscular level. This hi¬ 
erarchical organization is key to organizing a fan-out of commands from 
the higher to the lower levels of the hierarchy and is accompanied by a 
fan-in of sensor data from the muscular to the cortical level. We have 
tried to answer the question of how one might try to develop an envi¬ 
ronment of controllers for a multifingered robotic system so as to take 
into account this sort of hierarchical organization by way of a sample 
multi-robot control programming paradigm that we have developed here. 

3.3 Nonholonomic behavior in robotic systems 

In Chapter 6, we run into the question of how to deal with the presence 
of Pfaffian constraints when writing the dynamical equations of a mul¬ 
tifingered robot hand. In that chapter, we show how to incorporate the 
constraints into the Lagrangian equations. However, one question that 
is left unanswered in that chapter is the question of trajectory planning 
for the system with nonholonomic constraints. In the instance of a mul- 
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tifingered hand grasping an object, we give control laws for getting the 
grasped object to follow a specified position and orientation. However, 
if the fingertips are free to roll on the surface of the object, it is not 
explicitly possible for us to control the locations to which they roll us¬ 
ing only the tools of Chapter 6. In particular, we are not able to give 
control strategies for moving the fingers from one contact location to an¬ 
other. Motivated by this observation, we begin a study of nonholonomic 
behavior in robotic systems in Chapter 7. 

Nonholonomic behavior can arise from two different sources: bodies 
rolling without slipping on top of each other, or conservation of angular 
momentum during the motion. In this chapter, we expand our horizons 
beyond multifingered robot hands and give yet other examples of non¬ 
holonomic behavior in robotic systems arising from rolling: car parking, 
mobile robots, space robots, and a hopping robot in the flight phase. We 
discuss methods for classifying these systems, understanding when they 
are partially nonholonomic (or nonintegrable) and when they are holo- 
nomic (or integrable). These methods are drawn from some rudimentary 
notions of differential geometry and nonlinear control theory (controlla¬ 
bility) which we develop in this chapter. The connection between non- 
holonomy of Pfaffian systems and controllability is one of duality, as is 
explained in this chapter. 

Chapter 8 contains an introduction to some methods of motion plan¬ 
ning for systems with nonholonomic constraints. This is the study of 
trajectory planning for nonholonomic systems consistent with the con¬ 
straints on the system. This is a very rapidly growing area of research in 
robotics and control. We start with an overview of existing techniques 
and then we specialize to some methods of trajectory planning. We begin 
with the role of sinusoids in generating Lie bracket motions in nonholo¬ 
nomic systems. This is motivated by some solutions to optimal control 
problems for a simple class of model systems. Starting from this class 
of model systems, we show how one can generalize this class of model 
systems to a so-called chain form variety. We then discuss more general 
methods for steering nonholonomic systems using piecewise constant con¬ 
trols and also Ritz basis functions. We apply our methods to the examples 
presented in the previous chapter. We finally return to the question of 
dynamic finger repositioning on the surface of a grasped object and give 
a few different techniques for rolling fingers on the surface of a grasped 
object from one grasp to another. 

Chapter 9 contains a description of some of the growth areas in 
robotics from a technological point of view. From a research and an 
analytical point of view, we hope that the book in itself will convince 
the reader of the many unexplored areas of our understanding of robotic 
manipulation. 
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4 Bibliography 

It is a tribute to the vitality of the field that so many textbooks and books 
on robotics have been written in the last fifteen years. It is impossible to 
do justice or indeed to list them all here. We just mention some that we 
are especially familiar with and apologize to those whom we omit to cite. 

One of the earliest textbooks in robotics is by Paul [90], on the math¬ 
ematics, programming, and control of robots. It was followed in quick 
succession by the books of Gorla and Renaud [36], Craig [21], and Fu, 
Gonzalez and Lee [35]. The first two concentrated on the mechanics, dy¬ 
namics, and control of single robots, while the third also covered topics 
in vision, sensing, and intelligence in robots. The text by Spong and 
Vidyasagar [110] gives a leisurely discussion of the dynamics and control 
of robot manipulators. Also significant is the set of books by Coiffet [20], 
Asada and Slotine [2], and Koivo [52]. As this book goes to print, we are 
aware also of a soon to be completed new textbook by Siciliano and Sci- 
avicco. An excellent perspective of the development of control schemes 
for robots is provided by the collection of papers edited by Spong, Lewis 
and Abdallah [109]. 

The preceding were books relevant to single robots. The first mono¬ 
graph on multifingered robot hands was that of Mason and Salisbury [69], 
which covered some details of the formulation of grasping and substan¬ 
tial details of the design and control of the Salisbury three-fingered hand. 
Other books in the area since then have included the monographs by 
Cutkosky [22] and by Nakamura [79], and the collection of papers edited 
by Venkataraman and Iberall [116]. 

There are a large number of collections of edited papers on robotics. 
Some recent ones containing several interesting papers are those edited 
by Brockett [13], based on the contents of a short course of the American 
Mathematics Society in 1990; and a collection of papers on all aspects 
of manipulation edited Spong, Lewis, and Abdallah [109]; and a recent 
collection of papers on nonholonomic motion planning edited by Li and 
Canny [61], based on the contents of a short course at the 1991 IEEE 
International Conference on Robotics and Automation. 

Not included in this brief bibliographical survey are books on com¬ 
puter vision or mobile robots which also have witnessed a flourish of 
activity. 
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Chapter 2 

Rigid Body Motion 


A rigid motion of an object is a motion which preserves distance between 
points. The study of robot kinematics, dynamics, and control has at its 
heart the study of the motion of rigid objects. In this chapter, we provide 
a description of rigid body motion using the tools of linear algebra and 
screw theory. 

The elements of screw theory can be traced to the work of Chasles 
and Poinsot in the early 1800s. Chasles proved that a rigid body can 
be moved from any one position to any other by a movement consisting 
of rotation about a straight line followed by translation parallel to that 
line. This motion is what we refer to in this book as a screw motion. The 
infinitesimal version of a screw motion is called a twist and it provides a 
description of the instantaneous velocity of a rigid body in terms of its 
linear and angular components. Screws and twists play a central role in 
our formulation of the kinematics of robot mechanisms. 

The second major result upon which screw theory is founded concerns 
the representation of forces acting on rigid bodies. Poinsot is credited 
with the discovery that any system of forces acting on a rigid body can 
be replaced by a single force applied along a line, combined with a torque 
about that same line. Such a force is referred to as a wrench. Wrenches 
are dual to twists, so that many of the theorems which apply to twists 
can be extended to wrenches. 

Using the theorems of Chasles and Poinsot as a starting point, Sir 
Robert S. Ball developed a complete theory of screws which he published 
in 1900 [6]. In this chapter, we present a more modern treatment of the 
theory of screws based on linear algebra and matrix groups. The funda¬ 
mental tools are the use of homogeneous coordinates to represent rigid 
motions and the matrix exponential, which maps a twist into the corre¬ 
sponding screw motion. In order to keep the mathematical prerequisites 
to a minimum, we build up this theory assuming only a good knowledge 
of basic linear algebra. A more abstract version, using the tools of matrix 
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Lie groups and Lie algebras, can be found in Appendix A. 

There are two main advantages to using screws, twists, and wrenches 
for describing rigid body kinematics. The first is that they allow a global 
description of rigid body motion which does not suffer from singularities 
due to the use of local coordinates. Such singularities are inevitable when 
one chooses to represent rotation via Euler angles, for example. The sec¬ 
ond advantage is that screw theory provides a very geometric description 
of rigid motion which greatly simplifies the analysis of mechanisms. We 
will make extensive use of the geometry of screws throughout the book, 
and particularly in the next chapter when we study the kinematics and 
singularities of mechanisms. 


1 Rigid Body Transformations 

The motion of a particle moving in Euclidean space is described by 
giving the location of the particle at each instant of time, relative to 
an inertial Cartesian coordinate frame. Specifically, we choose a set of 
three orthonormal axes and specify the particle’s location using the triple 
(x,y,z) £ R 3 , where each coordinate gives the projection of the parti¬ 
cle’s location onto the corresponding axis. A trajectory of the particle is 
represented by the parameterized curve p(t) = ( x(t),y(t),z(t )) £ R 3 . 

In robotics, we are frequently interested not in the motion of individ¬ 
ual particles, but in the collective motion of a set of particles, such as the 
link of a robot manipulator. To this end, we loosely define a perfectly 
rigid body as a completely “undistortable” body. More formally, a rigid 
body is a collection of particles such that the distance between any two 
particles remains fixed, regardless of any motions of the body or forces 
exerted on the body. Thus, if p and q are any two points on a rigid body 
then, as the body moves, p and q must satisfy 

\\p{t) - q(t)\\ = ||p(0) - g(0)|| = constant. 

A rigid motion of an object is a continous movement of the particles 
in the object such that the distance between any two particles remains 
fixed at all times. The net movement of a rigid body from one location 
to another via a rigid motion is called a rigid displacement. In general, 
a rigid displacement may consist of both translation and rotation of the 
object. 

Given an object described as a subset O off 3 , a rigid motion of an 
object is represented by a continuous family of mappings g(t) : O —> R 3 
which describe how individual points in the body move as a function of 
time, relative to some fixed Cartesian coordinate frame. That is, if we 
move an object along a continuous path, g(t) maps the initial coordinates 
of a point on the body to the coordinates of that same point at time t. A 
rigid displacement is represented by a single mapping g : O —> R 3 which 
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maps the coordinates of points in the rigid body from their initial to final 
configurations. 

Given two points p,q £ O, the vector v £ R 3 connecting p to q is 
defined to be the directed line segment going from p to q. In coordinates 
this is given by v = q — p with p,q £ R 3 . Though both points and vec¬ 
tors are represented by 3-tuples of numbers, they are conceptually quite 
different. A vector has a direction and a magnitude. ( By the magni tude 
of a vector, we will mean its Euclidean norm, i.e., \Jv\ + v\ + n|.) It 
is, however, not attached to the body, since there may be other pairs of 
points on the body, for instance r and s with q — p = s — r, for which the 
same vector v also connects r to s. A vector is sometimes called a free 
vector to indicate that it can be positioned anywhere in space without 
changing its meaning. 

The action of a rigid transformation on points induces an action on 
vectors in a natural way. If we let g : O — * R 3 represent a rigid displace¬ 
ment, then vectors transform according to 

9*(v) = g(q) - g(p). 

Note that the right-hand side is the difference of two points and is hence 
also a vector. 

Since distances between points on a rigid body are not altered by rigid 
motions, a necessary condition for a mapping g : O —* R 3 to describe a 
rigid motion is that distances be preserved by the mapping. However, 
this condition is not sufficient since it allows internal reflections, which are 
not physically realizable. That is, a mapping might preserve distance but 
not preserve orientation. For example, the mapping (x, y, z) i—> (x, y, —z) 
preserves distances but reflects points in the body about the xy plane. 
To eliminate this possibility, we require that the cross product between 
vectors in the body also be preserved. We will collect these requirements 
to define a rigid body transformation as a mapping from R 3 to R 3 which 
represents a rigid motion: 

Definition 2.1. Rigid body transformation 

A mapping g : R 3 —> R 3 is a rigid body transformation if it satisfies the 
following properties: 

1. Length is preserved: \\g(p) —g(q)\\ = \\p — <?|| for all points p, q £ R 3 . 

2. The cross product is preserved: g*(v x w) = g*{v) x g*(w) for all 
vectors v,w £ R 3 . 

There are some interesting consequences of this definition. The first 
is that the inner product is preserved by rigid body transformations. One 
way to show this is to use the polarization identity , 

vfv 2 = |(||ui + v 2 \\ 2 - ||ui - v 2 \\ 2 ), 
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and the fact that 


IK + KI = IKK) + g*{v 2 )\\ IK - KI = IKK) - a* K)ll 

to conclude that for any two vectors v\,v 2 , 

vfv 2 = g*{vi) T g*(v 2 ). 

In particular, orthogonal vectors are transformed to orthogonal vectors. 
Coupled with the fact that rigid body transformations also preserve the 
cross product (property 2 of the definition above), we see that rigid body 
transformations take orthonormal coordinate frames to orthonormal co¬ 
ordinate frames. 

The fact that the distance between points and cross product between 
vectors is fixed does not mean that it is inadmissible for particles in a 
rigid body to move relative to each other, but rather that they can rotate 
but not translate with respect to each other. Thus, to keep track of the 
motion of a rigid body, we need to keep track of the motion of any one 
particle on the rigid body and the rotation of the body about this point. 
In order to do this, we represent the configuration of a rigid body by 
attaching a Cartesian coordinate frame to some point on the rigid body 
and keeping track of the motion of this body coordinate frame relative 
to a fixed frame. The motion of the individual particles in the body can 
then be retrieved from the motion of the body frame and the motion of 
the point of attachment of the frame to the body. We shall require that 
all coordinate frames be right-handed : given three orthonormal vectors 
x, y, z £ K 3 which define a coordinate frame, they must satisfy z = x x y. 

Since a rigid body transformation g : R 3 —> R 3 preserves the cross 
product, right-handed coordinate frames are transformed to right-handed 
coordinate frames. The action of a rigid transformation g on the body 
frame describes how the body frame rotates as a consequence of the 
rigid motion. More precisely, if we describe the configuration of a rigid 
body by the right-handed frame given by the vectors vi,v 2 ,v 2 attached 
to a point p, then the configuration of the rigid body after the rigid 
body transformation g is given by the right-handed frame of vectors 
<7*KK*KK*K) attached to the point g(p). 

The remainder of this chapter is devoted to establishing more detailed 
properties, characterizations, and representations of rigid body transfor¬ 
mations and providing the necessary mathematical preliminaries used in 
the remainder of the book. 


2 Rotational Motion in R ’ 

We begin the study of rigid body motion by considering, at the outset, 
only the rotational motion of an object. We describe the orientation of 
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Figure 2.1: Rotation of a rigid object about a point. The dotted coordi¬ 
nate frame is attached to the rotating rigid body. 


the body by giving the relative orientation between a coordinate frame 
attached to the body and a fixed or inertial coordinate frame. From now 
on, all coordinate frames will be right-handed unless stated otherwise. 
Let A be the inertial frame, B the body frame, and x a {,,y a b, z a b £ R 3 
the coordinates of the principal axes of B relative to A (see Figure 2.1). 
Stacking these coordinate vectors next to each other, we define a 3 x 3 
matrix: 

Rab — [xob y ab 2 a b\ ■ 

We call a matrix constructed in this manner a rotation matrix: every 
rotation of the object relative to the ground corresponds to a matrix of 
this form. 


2.1 Properties of rotation matrices 

A rotation matrix has two key properties that follow from its construc¬ 
tion. Let R £ R 3x3 be a rotation matrix and ri, , r '3 £ R 3 be its 

columns. Since the columns of R are mutually orthonormal, it follows 
that 


T 

r, r-j 


0 , if j 
1 , if * = j. 


As conditions on the matrix i?, these properties can be written as 


rr t = r t r = I. 


( 2 . 1 ) 


From this it follows that 

det R = ±1. 

To determine the sign of the determinant of R, we recall from linear 
algebra that 

det R = rf fa x r^). 
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Since the coordinate frame is right-handed, we have that r 2 x r 3 = ry so 
that det A = rjr 1 = 1. Thus, coordinate frames corresponding to right- 
handed frames are represented by orthogonal matrices with determinant 

1. The set of all 3 x 3 matrices which satisfy these two properties is 
denoted 50(3). The notation 50 abbreviates special orthogonal. Special 
refers to the fact that det R = +1 rather than ±1. 

More generally, we may define the space of rotation matrices in R nxn 
by 

SO(n) = {R£ R" xn : RR T = J,det R = + 1}. (2.2) 

We will be primarily interested in n = 3, although the n = 2 case (planar 
rotations) will also prove useful and is explored in the exercises. 

50(3) C R 3x3 is a group under the operation of matrix multiplication. 
A set G together with a binary operation o defined on elements of G is 
called a group if it satisfies the following axioms: 

1. Closure: If gy,g 2 G G 1 then gi o g 2 £ G. 

2. Identity: There exists an identity element, e, such that g o e = 
e o g = g ior every g £ G. 

3. Inverse: For each g £ G, there exists a (unique) inverse, c/ _1 £ G , 
such that g o g^ 1 = g _1 o g = e. 

4. Associativity: If gy, g 2 , £3 € G, then (gy o g 2 ) o g 3 = gy o (g 2 o g 3 ). 

In the instance of 50(3), note that 

1. If Ry,R 2 £ SO( 3), then RyR 2 £ SO(3) since 

RyR2^R\R2 )^ ~ RyR2R 2 Ry = R\Ry — I 
det(Rii? 2 ) = det(Ri) det(R 2 ) = +1- 

2. The identity matrix is the identity element. 

3. By equation (2.1) it follows that the inverse of R £ SO( 3) is R T £ 
SO(3). 

4. The associativity of the group operation follows from the associa¬ 
tivity of matrix multiplication; that is, {RyR 2 )Rz = Ry(R 2 Rs). 

Thus, SO( 3) is a group using the identity matrix I as the identity element 
and matrix multiplication as the group operation. We refer to SO(3) as 
the rotation group of R 3 . 

Every configuration of a rigid body that is free to rotate relative to 
a fixed frame can be identified with a unique R £ 50(3). Under this 
identification, the rotation group 50(3) is referred to as the configuration 
space of the system and a trajectory of the system is a curve R(t) £ 50(3) 
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for t £ [0, T], More generally, we shall call a set Q a configuration space 
for a system if every element x £ Q corresponds to a valid configuration 
of the system and each configuration of the system can be identified with 
a unique element of Q. 

A rotation matrix R £ SO{ 3) also serves as a transformation, taking 
coordinates of a point from one frame to another. Consider the point q 
shown in Figure 2.1. Let qb = ( Xb , yb, Zb) be the coordinates of q relative 
to frame B. The coordinates of q relative to frame A can be computed as 
follows: since Xb, yb , are projections of q onto the coordinate axes 

of B, which, in turn, have coordinates x a {,,y a b,Zab £ R 3 with respect to 
A , the coordinates of q relative to frame A are given by 

Qa — X a bXb T y abVb T Z a b~b- 
This can be rewritten as 


Qa — [ x ab y ab Z a b\ 


BabQb- 


In other words, R a b , when considered as a map from M 3 to R 3 , rotates 
the coordinates of a point from frame B to frame A. 

The action of a rotation matrix on a point can be used to define the 
action of the rotation matrix on a vector. Let Vb be a vector in the frame 
B defined as Vb = qb — Pb- Then, 

B a b[yb) -— BabQb B-abPb — Qa Pa — X a . 

Since matrix multiplication is linear, it may be verified that if Vb = qb — 
Pb = Sb — Tb then we still have that 

Bab$b Bab^b — B-abQb BabPb — X a 

and hence the action of R a b on a vector is well defined. 

Rotation matrices can be combined to form new rotation matrices 
using matrix multiplication. If a frame C has orientation Rb c relative to 
a frame B , and B has orientation R a b relative to another frame A , then 
the orientation of C relative to A is given by 

Rac = RabRbc- (2.3) 

Rac when considered as a map from R 3 to R 3 , rotates the coordinates of 
a point from frame C to frame A by first rotating from C to B and then 
from B to A. Equation (2.3) is the composition rule for rotations. 

A rotation matrix represents a rigid body transformation in the sense 
of the definition of the previous section. This is to say, it preserves 
distance and orientation. We prove this using some algebraic properties 


25 




of the cross product operation between two vectors. Recall that the cross 
product between two vectors a, b £ R 3 is defined as 


ax6 = 


a 2 b 3 

a 3 bi 

Oi & 2 


U 3 b 2 

0163 

a 2 bi 


Since the cross product by a is a linear operator, in axi may be 
represented using a matrix. Defining 


we can write 


' 0 

-a 3 

a 2 


a 3 

0 

-a x 

, (2-4) 

y«2 

ai 

0 


a x 6 = 

(a) A b. 


(2.5) 


We will often use the notation a as a replacement for ( a) A . 

Lemma 2.1. Given R £ SO( 3) and v,w £ R 3 , the following properties 
hold: 


R(v x w) = (Rv) x (Rw) (2.6) 

R(w) A R T = (Rw) A . (2.7) 

The first property in the lemma asserts that rotation by the matrix R 
commutes with the cross product operation; that is, the rotation of the 
cross product of two vectors is the cross product of the rotation of each 
of the vectors by R. The second property has an interpretation in terms 
of rotation of an instantaneous axis of rotation, which will become clear 
shortly. For now, we will merely use it as an algebraic fact. The proof of 
the lemma is by calculation. 

Proposition 2.2. Rotations are rigid body transformations 

A rotation R £ SO( 3) is a rigid body transformation; that is, 

1. R preserves distance: ||Rq — Rp\\ = \\q — p\\ for all q,p € R 3 . 

2. R preserves orientation: R(v x w) = Rv x Rw for all v,w £ R 3 . 
Proof. Property 1 can be verified by direct calculation: 

II Rq - Rp\\ 2 = (R(q - p)) T (R(q - p)) = (q- p) T R T R{q - p) 

= ( q-p) T {q~p) = I \q~p\\ 2 - 

Property 2 follows from equation (2.6). □ 
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Figure 2.2: Tip point trajectory generated by rotation about the w-axis. 


2.2 Exponential coordinates for rotation 

A common motion encountered in robotics is the rotation of a body about 
a given axis by some amount. For example, we might wish to describe the 
rotation of the link of a robot about a fixed axis, as shown in Figure 2.2. 
Let u> £ R 3 be a unit vector which specifies the direction of rotation and 
let 8 £ R be the angle of rotation in radians. Since every rotation of the 
object corresponds to some R £ SO( 3), we would like to write R as a 
function of u> and 6. 

To motivate our derivation, consider the velocity of a point q attached 
to the rotating body. If we rotate the body at constant unit velocity about 
the axis u, the velocity of the point, q, may be written as 

q{t) = ui x q(t) = Qq(t). (2.8) 


This is a time-invariant linear differential equation which may be inte¬ 
grated to give 


q(t) = e^q{ 0), 


where q(0 ) is the initial (t = 0) position of the point and e ut 
exponential 


e 


cut 


I + Ldt + 


fit ) 2 

2 ! 



is the matrix 


It follows that if we rotate about the axis uj at unit velocity for 8 units 
of time, then the net rotation is given by 


R[w,0) = e oe . 


(2.9) 


From its definition, it is easy to see that the matrix Q is a skew- 
symmetric matrix, i.e., it satisfies Q T = —Q. The vector space of all 3 x 3 
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skew matrices is denoted so(3) and more generally the space of n x n 
skew-symmetric matrices is 

so{n) = {Se R nx ” : S T = -S}. (2.10) 

(The reason for the notation so(n ) will become clear shortly.) As with 
S'O(n), the cases we are interested in are n = 2 and n = 3. We concen¬ 
trate on n = 3 here and explore n = 2 in the exercises. 

The set so(3) C R 3x3 is a vector space over the reals. Thus, the sum 
of two elements of so(3) is an element of so(3) and the scalar multiple of 
any element of so(3) is an element of so(3). Furthermore, we can identify 
so(3) with R 3 using the relationship (2.4) and the fact (v-|-i/;) A =v + w. 

It will be convenient to represent a skew-symmetric matrix as the 
product of a unit skew-symmetric matrix and a real number. Given a 
matrix u) £ so(3), ||u;|| = 1, and a real number 9 £ R, we write the 
exponential of u)9 as 

_ n2 q3 

exp (Q9) = e= I + 9Q + + ■ • ■ (2-11) 

Equation (2.11) is an infinite series and, hence, not useful from a compu¬ 
tational standpoint. To obtain a closed-form expression for exp(tD0), we 
make use of the following formulas for powers of a, which are verified by 
direct calculation. 


Lemma 2.3. Given a £ so(3), the following relations hold: 


a 1 = aa 1 — \\a\\ 2 I 
a 3 = — ||a|| 2 a 


( 2 . 12 ) 

(2.13) 


and higher powers of a can be calculated recursively. 

Utilizing this lemma with a = uid, ||o;|| = 1, equation (2.11) becomes 


0 3 9 5 

= 1 + 19 -^- + -- 


3! 5! 


9 2 9 4 9 6 

2! _ 4! + 6! 


and hence 


_j |_ ^ sin 0 + cD^ (1 _ cos 9) 


(2.14) 


This formula, commonly referred to as Rodrigues ’ formula , gives an effi¬ 
cient method for computing exp(cD0). When ||w|| ^ 1, it may be verified 
(see Exercise 12) that 


e Qf> = I - 


rrur sin (IMI0) + |£|p ( x - cos(||w||0)). 


We now verify that exp (u>9) is indeed a rotation matrix. 
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Proposition 2.4. Exponentials of skew matrices are orthogonal 

Given a skew-symmetric matrix Q G so(3) and 9 G R, 

e Q6 G 50(3). 

Proof. Defining R := exp (Q9), we must verify that R T R = I and det R = 
+1. To verify the first property, we have the following chain of equalities, 
which can be checked using equation (2.14), 


ujO 

— L JO cD T # 

DO 

e 

= e = e = 

e 


Thus f? -1 = R T and consequently R T R = I as desired. From this, it 
follows that det R = ±1. Using the continuity of the determinant as 
a function of the entries of a matrix, combined with continuity of the 
exponential map and the fact that detexp(O) = 1, we conclude that 
det R = +1. □ 


Proposition 2.4 asserts that the exponential map transforms skew- 
symmetric matrices into orthogonal matrices. Geometrically, the skew- 
symmetric matrix corresponds to an axis of rotation (via the mapping 
w w) and the exponential map generates the rotation corresponding 
to rotation about the axis by a specified amount 9. This relationship 
between skew-symmetric matrices and orthogonal matrices explains, in 
part, the notation so(3). We will now show that every rotation matrix 
can be represented as the matrix exponential of some skew-symmetric 
matrix; that is, the map exp : so(3) —> 50(3) is surjective (onto). 

Proposition 2.5. The exponential map is surjective onto 50(3) 

Given R G 50(3), there exists to G R 3 , ||o;|| = 1 and 9 G R such that 
R = exp(c o9). 

Proof. The proof is constructive. We equate terms of R and exp(wd) 
and solve the corresponding equations. By way of notation, we have the 
rotation matrix R to be 


R = 


r n 

r 12 

n 3 



r-2i 

02 

03 


(2.15) 

/31 

02 

03. 



cos 9, 

and 

S0 = 

sin0, write equation 

(2.14) 
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as 


e uB = I + a) sin# + a) 2 (l — cos0) 

1 - vo(u% + u> 3 ) ujiuj 2 ve - u) 3 se 


Wl UJ 2 Vg ■ 
_ LUiUzVg 

u\vg - 
UJlOJ 2 Vg H 
W 1 W 3 V 0 - 


I- W3S0 

- w 2 se 

- eg 

■ L0 3 Sg 

- Ul 2 Sg 


U)\U 3 Vg 
0J 2 U 3 Vg ■ 


■ U} 2 Sg 

■ U>i Sg 


1 - Vg(uf+U}l) 

U> 2 UJ 3 Vg + UiSg 1 — Vg(u>i + w|) 


UJiU) 2 Vg - UJ 3 Sg 
LO^Vg + Cg 
U> 2 UJ 3 Vg + LUiSg 


U}\UJ 3 Vg ■ 
U) 2 UJ 3 Vg ■ 


■ UJ 2 Sg 
u>i sg 


U 3 Vg + eg 


Equating (2.15) with (2.16), we see that 


(2.16) 


trace(i?) = rn + r 22 + r 33 = 1 + 2 cos 6. 


To verify that this equation has a solution, we recall that the trace of 
R is equal to the sum of its eigenvalues. Since R preserves lengths and 
det R = +1, its eigenvalues have magnitude 1 and occur in complex 
conjugate pairs (see Exercise 3). It follows that — 1 < trace(-R) < 3 and 
hence we can set 

_-y /trace(-R) — V 

V 2 


0 = cos 


(2.17) 


Note that there is an ambiguity in the value of 6 , in the sense that d±2nn 
or —6 ± 27rn could be chosen as well. 

Now, equating the off-diagonal terms of R and exp(tD0), we get 


r 32 ~ r 2 3 = 2uj 1 sg 
n 3 - r 3 i = 2 uj 2 sg 
r 2 i ~ r 12 = 2oj 3 sg. 


If 9 ^ 0, we choose 



r 32 - r -23 
r-is - r 3 1 
r 2 i - T12 


(2.18) 


Note that if 27 t — 6 had been chosen earlier in equation (2.17), the axis of 
rotation would have been — u>. Indeed, the exponential map is a many- 
to-one map from R 3 onto SO( 3). If R = 7, then trace(i?) = 3 and hence 
0 = 0 and lu can be chosen arbitrarily. If R ^ /, the above construction 
shows that there are two distinct ui and 9 £ [0, 27t) such that R = exp (Q8). 

□ 


The components of the vector u>9 £ R 3 given by equations (2.17) 
and (2.18) are called the exponential coordinates for R. Considering u> £ 
R 3 to be an axis of rotation with unit magnitude and 9 £ R to be an angle, 
Propositions 2.4 and 2.5 combine to give the following classic theorem. 
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Theorem 2.6 (Euler). Any orientation R € SO( 3) is equivalent to a 
rotation about a fixed, axis u> £ R 3 through an angle 9 £ [0,27r). 

This method of representing a rotation is also known as the equivalent 
axis representation. We note from the preceding proof that this repre¬ 
sentation is not unique since choosing to' = —to and 9 1 = 2n — 9 gives 
the same rotation as to and 9. Furthermore, if we insist that u> have unit 
magnitude, then ui is arbitrary for R = I (by choosing 0 = 0). The former 
problem is a consequence of the exponential map being many-to-one and 
the latter is referred to as a singularity of the equivalent axis represen¬ 
tation, alluding to the fact that one may lose smooth dependence of the 
equivalent axis as a function of the orientation R at R = I. 

2.3 Other representations 

The exponential coordinates are called the canonical coordinates of the 
rotation group. Other coordinates for the rotation group also exist and 
are briefly described below and in the exercises. We emphasize the con¬ 
nection of these other representations with the exponential coordinates 
presented above; more classical treatments of these representations can 
be found in standard kinematics texts. 

Euler angles 

One method of describing the orientation of a coordinate frame B relative 
to another coordinate frame A is as follows: start with frame B coincident 
with frame A. First, rotate the B frame about the 2 -axis of frame B (at 
this time coincident with frame A) by an angle a, then rotate about the 
(new) y-axis of frame B by an angle /3, and then rotate about the (once 
again, new) 2 -axis of frame B by an angle 7 . This yields a net orientation 
R a b{ot,(3, 7 ) and the triple of angles (cq/3, 7 ) is used to represent the 
rotation. 

The angles (a, /?, 7 ) are called the ZYZ Euler angles. Since all ro¬ 
tations are performed about the principal axes of the moving frame, we 
define the following elementary rotations about the x-, y-, and 2 -axes: 



'1 0 


0 

/4(0) := e 3 * = 

0 coscp 


- sin 4> 


0 sin <j> 


COS (f) 


cos /3 

0 

sin/3 

Ry(l 3 ) :=e^ = 

0 

1 

0 


— sin/3 

0 

cos /3 


cos a — 

sin a 0 

R z (a) := e z “ = 

sin a cos a 0 


0 

0 

1 
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and 








To derive the final orientation of frame B , it is easiest to derive the 
formula by viewing the rotation with B considered as the fixed frame, 
since then all rotations then occur around fixed axes. The appropriate 
sequence of rotations for the frame A, considering the B frame as fixed, 
is 

Rba = R z (-^)R y {-/3)R z (-a). 

Inverting this expression gives the rotation matrix of B relative to A: 

Rab = Rz(a)R y (/3)Rz(l) 

c a c /3 c 7 ~ S a Sry —C a Cf)S-y — S a Cj 

- S - t - CqS'y C(%Cry 

— SfjC-y SpSj 

Here c a ,s a are abbreviations for cos a and sin a, respectively, and simi¬ 
larly for the other terms. 

It is clear that any matrix of the form in equation (2.19) is an orthog¬ 
onal matrix (since it is a composition of elementary rotations). As in the 
case of the exponential map, the converse question of whether the map 
from (a,/3, 7 ) —> £>0(3) is surjective is an important one. The answer 
to this question is affirmative: given a rotation R £ SO( 3), the Euler 
angles can be computed by solving equation (2.19) for a, /3, and 7 . For 
example, when sin (3 ^ 0, the solutions are 

P = atan 2 (^/r | 1 +r§ 2 , r 33 ) 

a = atan 2 (r 23 /s /3 , r 13 /s 0 ) ( 2 - 20 ) 

7 = atan 2 (r 32 /s 0 , -r 3 i/s 0 ), 

where atan 2 (y,x) computes tan _ 1 (y/x) but uses the sign of both x and 
y to determine the quadrant in which the resulting angle lies. 

ZYZ Euler angles are an example of a local parameterization of 50(3). 
As in the case of the equivalent axis representation, singularities in the 
parameterization (referring to the lack of existence of global, smooth 
solutions to the inverse problem of determining the Euler angles from the 
rotation) occur at R = I, the identity rotation. In particular, we note 
that (a, /3, 7 ) of the form (a, 0, —a) yields R a b(a, 0, —a) = I. Thus, there 
are infinitely many representations of the identity rotation in the ZYZ 
Euler angles parameterization. 

Other types of Euler angle parameterizations may be devised by using 
different ordered sets of rotation axes. Common choices include ZYX axes 
(Fick angles) and YZX axes (Helmholtz angles). The ZYX Euler angles 
are also referred to as the yaw, pitch, and roll angles, with R a b defined by 
rotating about the x-axis in the body frame (roll), then the y-axis in the 
body frame (pitch), and finally the 2 -axis in the body frame (yaw). Both 
the ZYX and YZX Euler angle parameterizations have the advantage of 


c a s 0 
s a s 0 
C 0 . 


(2.19) 
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not having a singularity at the identity orientation, R = /, though they 
do contain singularities at other, different, orientations. For example, in 
the instance of ZYX Euler angles, we have: 

R ab = R^)Ry{B)R^) = 

which is singular when 6 = —7r/ 2 . It is a fundamental topological fact 
that singularities can never be eliminated in any 3 -dimensional represen¬ 
tation of SO( 3 ). This situation is similar to that of attempting to find a 
global coordinate chart on a sphere, which also fails. 

Quaternions 

Quaternions generalize complex numbers and can be used to represent 
rotations in much the same way as complex numbers on the unit circle can 
be used to represent planar rotations. Unlike Euler angles, quaternions 
give a global parameterization of SO( 3 ), at the cost of using four numbers 
instead of three to represent a rotation. 

Formally, a quaternion is a vector quantity of the form 

Q = q o + Qi' 1 + 92 j + 93k qi £K,i = 0 ,..., 3 , 

where 90 is the scalar component of Q and 9 = (91,(72,93) is the vector 
component. A convenient shorthand notation is Q = (90, 9) with 90 £ R, 
q £ R 3 . The set of quaternions Q is a 4 -dimensional vector space over 
the reals and forms a group with respect to quaternion multiplication, 
denoted Multiplication is distributive and associative, but not com¬ 
mutative; it satisfies the relations 

ai = ia aj = ja ak = ka aeR 

i i = j • j = k ■ k = i • j • k = -1 
i j =-j i = k j • k = —k j = i k i = -i k = j 

The conjugate of a quaternion Q = (90, q) is given by Q* = (90, —q) and 
the magnitude of a quaternion satisfies 

IIQII 2 = Q ■ Q* = 9 o + qj + 9I + ? 3 - 

It is straightforward to verify that the inverse of a quaternion is Q -1 = 
Q 7 HQH 2 and that Q = ( 1 , 0 ) is the identity element for quaternion mul¬ 
tiplication. 

The product between two quaternions has a simple form in terms of 
the inner and cross products between vectors in R 3 . Let Q = (90, 9) and 
P = ( Po,P ) be quaternions, where 90 ,po G R are the scalar parts of Q 
and P and q 7 p are the vector parts. It can be shown algebraically that 
the product of two quaternions satisfies: 

Q ■ P = (90P0 - 9 • P, qoP + PoQ + q X P ). 
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In most applications, this formula eliminates the need to make direct use 
of the multiplicative relations given above. 

The unit quaternions are the subset of all Q € Q such that ||Q|| = 
1. The unit quaternions also form a group with respect to quaternion 
multiplication (Exercise 6). Given a rotation matrix R = exp(u)0), we 
define the associated unit quaternion as 

Q = (cos(0/2), wsin(0/2)), 

where u> £ R 3 represents the unit axis of rotation and 9 £ R. represents 
the angle of rotation. A detailed calculation shows that if Q a b represents 
a rotation between frame A and frame B , and Qb c represents a rotation 
between frames B and C, then the rotation between A and C is given by 
the quaternion 

Qac — Qab ‘ Qbc- 

Thus, the group operation on unit quaternions directly corresponds to 
the group operation for rotations. Given a unit quaternion Q = ( qo,q ), 
we can extract the corresponding rotation by setting 

1 f • if 9 A 0, 

9 = 2 cos -1 g 0 w=^ sm ( e / 2 ) ^ ’ 

I 0 otherwise, 


and R = exp(u) 9). 

Since the group structure for quaternions directly corresponds to that 
of rotations, quaternions provide an efficient representation for rotations 
which do not suffer from singularities. Their properties are explored more 
fully in the exercises. 


3 Rigid Motion in K 3 

Recall from Section 1 that a rigid motion is one that preserves the dis¬ 
tance between points and the angle between vectors. We represent rigid 
motions by using rigid body transformations to describe the instanta¬ 
neous position and orientation of a body coordinate frame relative to 
an inertial frame. This representation relies on the fact that rigid body 
transformations map right-handed, orthonormal frames to right-handed, 
orthonormal frames, thus preserving distance and angles. In this book 
we refer to all transformations between coordinate frames as rigid body 
transformations (or just rigid transformations), whether or not a rigid 
body is explicitly present. 

In general, rigid motions consist of rotation and translation. In the 
preceding section, we discussed representations of pure rotational motion. 
The procedure for representing pure translational motion is very simple: 
choose a (any) point in the body and keep track of the coordinates of the 
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Figure 2.3: Coordinate frames for specifying rigid motions. 


point relative to some known frame. This gives a curve p(t) £ R 3 ,t £ 
[0, T], for a trajectory of the entire rigid body. 

The representation of general rigid body motion, involving both trans¬ 
lation and rotation, is more involved. We describe the position and ori¬ 
entation of a coordinate frame B attached to the body relative to an 
inertial frame A (see Figure 2.3). Let p a b £ R 3 be the position vector 
of the origin of frame B from the origin of frame A, and R a b £ 50(3) 
the orientation of frame 5, relative to frame A. A configuration of the 
system consists of the pair ( p a b,R a b)i and the configuration space of the 
system is the product space of R 3 with 50(3), which shall be denoted as 
55(3) (for special Euclidean group): 

55(3) = {(p,R) : P £R 3 1 R£ 50(3)} = R 3 x 50(3). (2.21) 

We defer the proof of the fact that 55(3) is a group to the next subsection. 
As in the case of 50(3), there is a generalization to n dimensions, 

55(n) := R" x SO(n). 

Analogous to the rotational case, an element (p, R) £ 55(3) serves as 
both a specification of the configuration of a rigid body and a transforma¬ 
tion taking the coordinates of a point from one frame to another. More 
precisely, let q a ,Qb G R 3 be the coordinates of a point q relative to frames 
A and 5, respectively. Given qb, we can find q a by a transformation of 
coordinates: 

qa = Pab T Rabqb (2.22) 

where g a b = ( p a b, Rab ) £ SE(3) is the specification of the configuration of 
the B frame relative to the A frame. By an abuse of notation, we write 
g{q) to denote the action of a rigid transformation on a point, 

g{q) =p + Rq, 
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so that q a = g a b{qb)- 

The action of a rigid transformation g = ( p , R) on a vector v = s — r 
is defined by the following formula: 

g*(v) ■■= g(s) - g(r) = R(s - r) = i?u. 

Thus, a vector is transformed by rotation. 


3.1 Homogeneous representation 


The transformation of points and vectors by rigid transformations has a 
simple representation in terms of matrices and vectors in R 4 . We begin 
by adopting some notation. We append 1 to the coordinates of a point 
to yield a vector in R 4 , 

' ii ' 

n - 92 

g — 93 • 

. i _ 

These are called the homogeneous coordinates of the point q. Thus, the 
origin has the form 

'o' 

n - o 
— 0 . 

. 1 . 

Vectors, which are the difference of points, then have the form 


v = 


'Vl ' 

V 2 

■»3 

L o J 


Note that the form of the vector is different from that of a point. The 
0 and 1 in the fourth component of vectors and points, respectively, will 
remind us of the difference between points and vectors and enforce a few 
rules of syntax: 


1. Sums and differences of vectors are vectors. 

2. The sum of a vector and a point is a point. 

3. The difference between two points is a vector. 

4. The sum of two points is meaningless. 


The transformation q a = g a b(qb) given in equation (2.22) is an affine 
transformation. Using the preceding notation for points, we may repre¬ 
sent it in linear form by writing it as 


q a 


Rab 

Pab 


qb 

l 


0 

1 


l 


gabqb- 


The 4x4 matrix g a t> is called the homogeneous representation of g a b G 
SE( 3). In general, if g = (p,R) G SE( 3), then 


9 = 


R 

0 


P 

1 


(2.23) 
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The price to be paid for the convenience of having a homogeneous or linear 
representation of the rigid body motion is the increase in the dimension 
of the quantities involved from 3 to 4. 

The last row of the matrix of equation (2.23) appears to be “extra 
baggage” as well. However, in the graphics literature, the number 1 is 
frequently replaced by a scalar constant which is either greater than 1 
to represent dilation or less than 1 to represent contraction. Also, the 
row vector of zeros in the last row may be replaced by some other row 
vector to provide “perspective transformations.” In both these instances, 
of course, the transformation represented by the augmented matrix no 
longer corresponds to a rigid displacement. 

Rigid body transformations can be composed to form new rigid body 
transformations. Let g^c £ SE( 3) be the configuration of a frame C 
relative to a frame B, and g a b the configuration of frame B relative to 
another frame A. Then, using equation (2.23), the configuration of C 
relative to frame A is given by 


9ac — gab gbc — 


RabR 


be 


o 


RabPbc T Pab 
1 


(2.24) 


Equation (2.24) defines the composition rule for rigid body transforma¬ 
tions to be the standard matrix multiplication. Using the homogeneous 
representation, it may be verified that the set of rigid transformations is 
a group; that is: 


1 . 

2 . 

3. 


If 51,52 £ SE( 3), then gig 2 £ SE( 3). 

The 4x4 identity element, /, is in SE(3). 

If g £ SE(3), then the inverse of g is determined by straightforward 
matrix inversion to be: 




R t ~R t p 


£ SE{ 3) 


so that g 1 = (— R T p , R T ). 

4. The composition rule for rigid body transformations is associative. 


Using the homogeneous representation for a vector v = s—r, we obtain 
the representation for a rigid body transformation of v by multiplying the 
homogeneous representations of v by the homogeneous representation of 


g*v = g(s) - g(r) = 


Note that by defining the homogeneous representation of a vector to have 
a zero in the bottom row, we are able to once again use matrix multi¬ 
plication to represent the action of a rigid transformation, this time on 
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Figure 2.4: Rigid body motion generated by rotation about a fixed axis. 


a vector instead of a point. For notational simplicity, in what follows 
we will confuse homogeneous representations and the abstract represen¬ 
tation of points, vectors, and rigid body transformations. Thus, we will 
write gq and gv instead of gq and g*v. 

The next proposition establishes that elements of SE{ 3) are indeed 
rigid body transformations; namely, that they preserve angles between 
vectors and distances between points. 

Proposition 2.7. Elements of SE( 3) represent rigid motions 

Any g £ SE( 3) is a rigid body transformation: 

1. g preserves distance between points: 

II gq - gp\\ = \\q-p\\ for all points q,p £ R 3 . 

2. g preserves orientation between vectors: 

g*(v x w) = g*v x g*w for all vectors v,w £ R 3 . 

Proof. The proofs follow directly from the corresponding proofs for rota¬ 
tion matrices: 


II 591 - gq 2 II = \\Rqi - Rq 2 \\ = ||<?i - 92 1| 

g*v x g*w = Rv x Rw = R(v x w). 


□ 


Example 2.1. Rotation about a line 

Consider the motion of a rigid body rotated about a line in the z direction, 
through the point (0, l \, 0) £ R 3 , as shown in Figure 2.4. If we let 8 denote 
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the amount of rotation, then the orientation of coordinate frame B with 
respect to A is 


Bab 


cos 0 — sin 0 0 

sin 6 cos 6 0 

0 0 1 


The coordinates for the origin of frame B are 


Pab = 


0 

h 

0 


again relative to frame A. The homogeneous representation of the con¬ 
figuration of the rigid body is given by 


9ab(0) 


cos 9 — sin 0 0 0 

sin0 cos0 0 l\ 

0 0 10 

0 0 0 1 


Note that when the angle 0 = 0, g a b( 0) gives that the relative displace¬ 
ment between the two frames is a pure translation along the y- axis. 


3.2 Exponential coordinates for rigid motion and twists 

The notion of the exponential mapping introduced in Section 2 for SO( 3) 
can be generalized to the Euclidean group, SE( 3). We will make extensive 
use of this representation in the sequel since it allows an elegant, rigorous, 
and geometric treatment of spatial rigid body motion. We begin by 
presenting a pair of motivational examples and then present a formal set 
of definitions. 

Consider the simple example of a one-link robot as shown in Fig¬ 
ure 2.5a, where the axis of rotation is a; £ M 3 , ||w|| = 1, and q £ R 3 is 
a point on the axis. Assuming that the link rotates with unit velocity, 
then the velocity of the tip point, pit), is 

pit) = U) x (p(t) - q). (2.25) 


This equation can be conveniently converted into homogeneous coordi¬ 
nates by defining the 4x4 matrix £ to be 


£ = 


U! 

0 


v 

0 ’ 


(2.26) 


with v = — oj x q. Equation (2.25) can then be rewritten with an extra 
row appended to it as 


p 


OJ 

—oj x q 


p 

0 


0 

0 


i 


P = &■ 
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The solution of the differential equation is given by 

p(t) = e&p( 0), 

where is the matrix exponential of the 4x4 matrix £f, defined (as 
usual) by 

e& = , +a+ <|! + (|! + ... 

The scalar t is the total amount of rotation (since we are rotating with 
unit velocity). exp(£f) is a mapping from the initial location of a point 
to its location after rotating t radians. 

In a similar manner, we can represent the transformation due to trans¬ 
lational motion as the exponential of a 4 x 4 matrix. The velocity of a 
point attached to a prismatic joint moving with unit velocity (see Fig¬ 
ure 2.5b) is 

p(t) = v. (2.27) 

Again, the solution of equation (2.27) can be written as exp(£f)p(0), 
where t is the total amount of translation and 


0 v 


0 0 


(2.28) 


The 4x4 matrix £ given in equations (2.26) and (2.28) is the gen¬ 
eralization of the skew-symmetric matrix cD £ so(3). Analogous to the 
definition of so(3), we define 

se(3) := {(u,a)) : v £ R 3 ,tD £ so(3)}. (2.29) 

In homogeneous coordinates, we write an element £ £ se(3) as 
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An element of se(3) is referred to as a twist , or a (infinitesimal) generator 
of the Euclidean group. We define the V (vee) operator to extract the 
6 -dimensional vector which parameterizes a twist, 


(2.30) 


and call £ := (v, w) the twist coordinates of £. The inverse operator, A 
(wedge), forms a matrix in se(3) out of a given vector in M 6 : 


id 

V 

V 

V 

0 

0 


LO 


r - 

A 


I 

V 


LO 

V 

LO 


0 

0 


(2.31) 


Thus, (el 6 represents the twist coordinates for the twist £ € se(3); this 
parallels our notation for skew-symmetric matrices. 

Proposition 2.8. Exponential map from se(3) to SE(3) 

Given t; £ se(3) and 6 £ R, the exponential of £6 is an element of SE(3), 
i.e., 

e& € SE{ 3). 

Proof. The proof is by explicit calculation. In the course of the proof, we 
will obtain a formula for exp(£(9). Write £ as 


£ = 


LO V 

0 0 


Case 1 [lo = 0). If lo = 0, then a straightforward calculation shows that 

^ = ^ = f 4 = ... = o 

so that exp(£(9) = I + fd and hence 


e^ = 


I vO 
0 1 


u> = 0 


(2.32) 


which is in SE( 3) as desired. 


Case 2(w / 0). Assume ||w|| = 1, by appropriate scaling of 6 if necessary, 
and define a rigid transformation g by 


9 = 


I to X V 

0 1 


(2.33) 


Now, using the calculation of Lemma 2.3, with |jw|| = 1, we have 

£' = g-% 


/ —LO X V 


LO V 


/ LO X V 

0 1 


0 0 


0 1 


(2.34) 


LO 

T 

LOLO V 


LO 

hui 

0 

0 


0 

0 
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where h := ou T v. Using the following identity (see Exercise 8), 


_ e s(? 8)g _ ge^ 0 g 


(2.35) 


it suffices to calculate exp(£'0). This simplifies the calculation since it 
may be verified (using lulu = lu x uu = 0) that 


(O 2 


'lu 2 O' 

0 0 ’ 


(O 3 


w 3 O' 

0 0 ’ 


Hence, 



huu6 
1 ’ 


and using equation (2.35) it follows that 



(/ — e ue )(u x v) + lulu t v9 

1 


u / 0 


(2.36) 


which is an element of SE( 3). □ 

The transformation g = exp(£0) is slightly different than the rigid 
transformations that we have encountered previously. We interpret it 
not as mapping points from one coordinate frame to another, but rather 
as mapping points from their initial coordinates, p(0 ) £ R 3 , to their 
coordinates after the rigid motion is applied: 


p(0) = e &p( 0). 


In this equation, both p(0) and p{6) are specified with respect to a single 
reference frame. Similarly, if we let g a b{ 0) represent the initial configu¬ 
ration of a rigid body relative to a frame A, then the final configuration, 
still with respect to A, is given by 

g a b(0) = e&g ab ( 0). (2.37) 


Thus, the exponential map for a twist gives the relative motion of a rigid 
body. This interpretation of the exponential of a twist as a mapping from 
initial to final configurations will be especially important as we study the 
kinematics of robot mechanisms in the next chapter. 

Our primary interest is to use the exponential map as a representation 
for rigid motion, and hence we must show that every rigid transformation 
can be written as the exponential of some twist. The following proposition 
asserts that this is always possible and gives a constructive procedure for 
finding the twist which generates a given rigid transformation. 

Proposition 2.9. Surjectivity of the exponential map onto SE( 3) 

Given g £ SE( 3), there exists £ £ se( 3) and such that g = exp (£0). 
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Proof. (Constructive). Let g = ( R,p ) with R £ SO( 3), p £ R 3 . We 
ignore the trivial case ( R,p ) = (1,0) which is solved with 0 = 0 and 
arbitrary £. 

Case 1 (R = I). If there is no rotational motion, set 


£ = 


0 

0 



0 = Ibll- 


Equation (2.32) verifies that exp(£0) = ( I,p) = g. 

Case 2 {R ^ I). To find £ = (v,ui), we equate exp(£0) and g and solve 
for v,u>. Using equation (2.36): 


e 


& _ 



(/ — e u9 ){u} x v) + luu> t v9 

1 


oj and 9 are obtained by solving the rotation equation exp(£50) = R , as 
in Proposition 2.5 of the previous section. This leaves the equation 

(J — e u9 )(uj x v) + uju) T v6 = p, (2.38) 

which must be solved for v. It suffices to show that the matrix 


A= (I — e u9 )tjj + 


is nonsingular for all 9 £ (0,27r). This follows from the fact that the two 
matrices which comprise A have mutually orthogonal null spaces when 
9^0 (and R^ I). Hence, Av = 0 <*=> v = 0. See Exercise 9 for more 
details. □ 

In light of Proposition 2.9, every rigid transformation g can be written 
as the exponential of some twist f9 £ se( 3). We call the vector £9 £ M. 6 
the exponential coordinates for the rigid transformation g. Note that, as 
in the case of rotations, the mapping exp : se(3) —> SE( 3) is many-to-one 
since the choice of to and 9 for solving the rotational component of the 
motion is not unique. This does not present great difficulties since for 
most applications we are given the twist as part of the problem and we 
wish to find the corresponding rigid motion. 


Example 2.2. Twist coordinates for rotation about a line 

Consider the rigid displacement generated by rotating about a fixed axis 
in space, as shown in Figure 2.6. The configuration of the B frame is 
given by 


cos a 
sin a 
0 
0 


— sin a 


0 —/2sina 

0 h+h cos a 

1 0 

0 1 


9ab — 


cos a 
0 
0 
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Figure 2.6: Rigid body displacement generated by rotation about a fixed 
axis. 


We wish to calculate the twist coordinates corresponding to the configu¬ 
ration of the frame B relative to frame A. 

To compute the twist which generates g a b, we follow the proof of 
Proposition 2.9, assuming a ^ 0 (so that R ^ I). The axis u> £ M 3 and 
angle 9 £ R which satisfy exp(cD0) = R a b are 


9 = a, 


since we are rotating about the 2 -axis. To find v , we must solve 


(j — uj + wlo t 0 


v = Pab- 


Using the fact that 9 = a and expanding the left-hand side, this equation 
becomes 


sin a 

cos a — 1 

O' 


—I 2 sin a 

1 — cos a 

sin a 

0 

V = 

l\ + I 2 cos a 

0 

0 

a 


0 


The solution is given by 

1 

2 

sin a 

2(1 —cos a) 

o 



—12 sin a 


r h—i 2 

2 


Zi + I2 cos a 


(Z1+J2) sin ck 


0 


2(1 — cos a) 

0 


sin q 

2(1 —cos a) 
~2 


Thus, the twist coordinates for g a j, are 


£ = 


l l~ l 2 2 

2 

(l ~\~l 2 ) sin q 
2(1 —cos a) 

0 

0 

0 

1 


9 = a ^7 0. 
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(a) general screw 


(b) pure translation 


Figure 2.7: Screw motions. 


This solution may be unexpected, considering that the motion was 
generated by a pure rotation about an axis. The reason for the compli¬ 
cated form of the solution is that we took the exponential coordinates 
of the absolute transformation between the B and A coordinate frames. 
Consider, instead, the exponential coordinates for the relative transfor¬ 
mation 


ff(«) =9ab(a)gJ( 0), 


where g a b(0 ) is the transformation corresponding to a = 0 (a pure trans¬ 
lation) . It can be verified that the exponential coordinates for the relative 
transformation g(a) are 



£= 8 e = a^o. 


o 


L l J 


3.3 Screws: a geometric description of twists 

In this section, we explore some of the geometric attributes associated 
with a twist £ = (v,u>). These attributes give additional insight into the 
use of twists to parameterize rigid body motions. We begin by defining a 
specific class of rigid body motions, called screw motions, and then show 
that a twist is naturally associated with a screw. 

Consider a rigid body motion which consists of rotation about an 
axis in space through an angle of 9 radians, followed by translation along 
the same axis by an amount d as shown in Figure 2.7a. We call such a 
motion a screw motion , since it is reminiscent of the motion of a screw, in 
so far as a screw rotates and translates about the same axis. To further 
encourage this analogy, we define the pitch of the screw to be the ratio 
of translation to rotation, h := d/9 (assuming 9^0). Thus, the net 
translational motion after rotating by 9 radians is hO. We represent the 
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Figure 2.8: Generalized screw motion (with nonzero rotation). 


axis as a directed line through a point; choosing q £ R 3 to be a point on 
the axis and u> £ R 3 to be a unit vector specifying the direction, the axis 
is the set of points 

l = {q + Xu; : A e R}. (2.39) 

The above definitions hold when the screw motion consists of a nonzero 
rotation followed by translation. 

In the case of zero rotation, the axis of the screw must be defined dif¬ 
ferently: we take the axis as the line through the origin in the direction 
v (i.e., v is a vector of magnitude 1), as shown in Figure 2.7b. By con¬ 
vention, the pitch of this screw is oo and the magnitude is the amount of 
translation along the direction v. Collecting these, we have the following 
definition of a screw: 

Definition 2.2. Screw motion 

A screw S consists of an axis l , a pitch h, and a magnitude M. A screw 
motion represents rotation by an amount 9 = M about the axis l followed 
by translation by an amount hd parallel to the axis l. If h = oo then the 
corresponding screw motion consists of a pure translation along the axis 
of the screw by a distance M. 

To compute the rigid body transformation associated with a screw, 
we analyze the motion of a point p £ R 3 , as shown in Figure 2.8. The 
final location of the point is given by 

gp = q + e uj9 (p — q) + hOu> 
or, in homogeneous coordinates, 


p 


- e Q0 (j _ e ue^ q + h6u} - 

P 

1 


0 1 

1 
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Since this relationship must hold for all p £ R , the rigid motion given 
by the screw is 


e ae (I - e Q0 )q + hOui 

0 1 


(2.40) 


As in the last section, this transformation maps points attached to the 
rigid body from their initial coordinates (9 = 0) to their final coordinates, 
and all points are specified with respect to the fixed reference frame. 

Note that the rigid body displacement given in equation (2.40) has 
the same form as the exponential of a twist, given in equation (2.36): 


e 


& _ 



(/ — e u9 ){uj x v) + utjj T vO 

1 


In fact, if we choose v = —u> x q + hto , then £ = (v,u) generates the 
screw motion in equation (2.40) (assuming ||w|| = 1, 9 ^ 0). In the case 
of a pure rotation, h = 0 and the twist associated with a screw motion 
is simply £ = (—uj x q,ui). In the instance that the screw corresponds 
to pure translation, we let 9 be the amount of translation, and the rigid 
body motion described by this “screw” is 


9 = 


I 

0 


9v 
1 ’ 


(2.41) 


which is precisely the motion generated by exp(£(9) with £ = ( v , 0). Thus, 
we see that a screw motion corresponds to motion along a constant twist 
by an amount equal to the magnitude of the screw. 

In fact, we can go one step further and define a screw associated 
with every twist. Let £ £ se(3) be a twist with twist coordinates £ = 
(v,u}) £ R 6 . We do not assume that ||w|| = 1, allowing both translation 
plus rotation as well as pure translation. The following are the screw 
coordinates of a twist: 


1. Pitch: 


h = 


T 

U) V 


12 ' 


(2.42) 


The pitch of a twist is the ratio of translational motion to rotational 
motion. If u> = 0, we say that £ has infinite pitch. 


2 . Axis: 


l = 


{f^+Aca:A£K}, 
{0 + Aw : A £ M}, 


if uj ^ 0 
if co = 0. 


(2.43) 


The axis l is a directed line through a point. For to ^ 0, the axis is 
a line in the u> direction going through the point For u> = 0, 

the axis is a line in the v direction going through the origin. 
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3. Magnitude: 


IM|, ifw^O 
IM|, if Cl; = O. 


(2.44) 


The magnitude of a screw is the net rotation if the motion contains 
a rotational component, or the net translation otherwise. If we 
choose |M| = 1 (or ||i;|| = 1 when co = 0), then a twist £ 6 has 
magnitude M = 0. 


We next show that given a screw, we can define a twist which realizes 
the screw motion and has the proper geometric attributes. It suffices to 
prove that we can define a twist with a given set of attributes, since any 
twist with those attributes will generate the correct screw motion. 


Proposition 2.10. Screw motions correspond to twists 

Given a screw with axis l, pitch h, and magnitude M, there exists a unit 
magnitude twist £ such that the rigid motion associated with the screw is 
generated by the twist 


Proof. The proof is by construction. We split the proof into the usual 
cases: pure translation and translation plus rotation. For consistency, we 
generate a screw of the form £0, where 9 = M. We will assume that q is 
a point on the axis of the screw. 

Case 1 (h = oo). Let l = {q + Xv : ||i;|| = 1, A £ R}, 9 = M, and define 




0 v 
0 0 


The rigid body motion exp(£(9) corresponds to pure translation along the 
screw axis by an amount 9. 

Case 2 (h finite). Let l = {q + Xco : ||w|| = 1, A € R}, 9 = M, and define 

Q —co xg| hco 

0 0 



The fact that the rigid body motion exp (£9) is the appropriate screw 
motion is verified by direct calculation. JJJ 

There are several important special cases of screw motion of which we 
shall make frequent use. A zero pitch screw is a screw motion for which 
h = 0, corresponding to a pure rotation about an axis. Zero pitch screws 
are used to model the action of a revolute joint of a manipulator. The axis 
of the screw corresponds to the axis of rotation of the joint. An infinite 
pitch screw is a motion for which h = oo, as previously mentioned. This 
case corresponds to a pure translation and is the model for the action of 
a prismatic joint. The axis of the screw is defined to be a line through 
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the origin which points in the direction of translation (a line through any 
other point could also be used). The magnitude of the screw gives the 
amount of the displacement. Finally, we define a unit twist to be a twist 
such that either ||u>||=l, oru> = 0 and ||v|| = 1; that is, a unit twist has 
magnitude M = 1. Unit twists are useful since they allow us to express 
rigid motions due to revolute and prismatic joints as g = exp(£0), where 
6 corresponds to the amount of rotation or translation. 

Some comments about the point q on the axis of the screw in the 
formulas above are in order. For instance, it is important to note that 
the formulas do not change for different choices of points on the axis of 
the screw. Thus, if q' = q + Xuj is some other point on the axis of the 
screw, the formula in equation (2.40) would be unchanged. It is also 
instructive to verify that for points on the axis of the screw, the screw 
motion is purely translational of magnitude hQ , as may be verified by 
applying equation (2.40) to points on the axis. 

The geometric meaning of a screw is expressed succinctly in the follow¬ 
ing theorem. Its proof follows directly from the definition of the attributes 
of a twist. 

Theorem 2.11 (Chasles). Every rigid, body motion can be realized by a 
rotation about an axis combined with a translation parallel to that axis. 

As mentioned previously, it is important to keep in mind that the 
exponential of a twist represents the relative motion of a rigid body. As a 
mapping, exp(£0) takes points from their initial coordinates, p(0) £ R 3 , 
to their coordinates after the rigid motion is applied: 

p(9) = e&p( 0). 

Both p(0) and p(6) are specified with respect to a single reference frame. 
If a coordinate frame B is attached to a rigid body undergoing a screw 
motion, the instantaneous configuration of the coordinate frame B , rela¬ 
tive to a fixed frame A , is given by 

g a b( 0 ) = e&g ab ( 0). (2.45) 

This transformation can be interpreted as follows: multiplication by 
g a b{ 0) maps the coordinates of a point relative to the B frame into A’s 
coordinates, and the exponential map transforms the point to its final 
location (still in A coordinates). 

Example 2.3. Rotation about a line 

Consider the motion of a rigid body rotating about a fixed axis in space, 
as shown in Figure 2.9. This motion corresponds to a zero-pitch screw 
about an axis in the u> = (0, 0,1) direction passing through the point 
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Figure 2.9: Rigid body motion generated by rotation about a fixed axis. 


q = (0, Zi, 0). The corresponding twist is 


£ = 


— u> x q 

LO 


The exponential of this twist is given by 
- uS (J_e29)( wXt ,) 




0 


1 


cos 8 
sin 8 
0 
0 


o 

0 

0 

0 

1 J 


— sin 8 0 

cos 8 0 

0 1 

0 0 


1 1 sin0 
Zi (1 — cos0) 
0 
1 


When applied to the homogeneous representation of a point, this matrix 
maps the coordinates of a point on the rigid body, specified relative to the 
frame A with 8 = 0, to the coordinates of the same point after rotating 
by 8 radians about the axis. 

The rigid transformation which maps points in B coordinates to A 
coordinates—and hence describes the configuration of the rigid body—is 
given by g a b{0) = exp(£8)g ab (0) where 


gab ( 0 ) 


I 

roii 

h 


0 J 

0 

1 


Taking the exponential and performing the matrix multiplication yields 


cos 8 — sin 0 0 0 

sin0 cos 8 0 l\ 

9ab= 0 0 10 ’ 

0 0 0 1 

which can be verified by inspection. 
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4 Velocity of a Rigid Body 

In this section, we derive a formula for the velocity of a rigid body whose 
motion is given by g(t ), a curve parameterized by time t in 55(3). This 
is not such a naive question as in the case of a single particle following 
a curve q(t) £ R 3 , where the velocity of the particle is v q (t) = jgq(t), 
because this notion of velocity cannot be generalized since SE( 3) is not 
Euclidean. In particular, the quantity g(t) (j SE( 3) and g(t) ^ se(3) and 
the question of its connection with rotational and translational velocity 
needs to be handled with care. Further, the definition of velocity needs to 
relate to our informal understanding of rotational and translational ve¬ 
locity. We will show that the proper representation of rigid body velocity 
is through the use of twists. 

4.1 Rotational velocity 

Consider first the case of pure rotational motion in R 3 . Let R a b{t) £ 
50(3) be a curve representing a trajectory of an object frame B, with 
origin at the origin of frame A, but rotating relative to the fixed frame 
A. We call A the spatial coordinate frame and B the body coordinate 
frame . 1 Any point q attached to the rigid body follows a path in spatial 
coordinates given by 

Qa(t') — Bab{t)qb- 

Note that the coordinates qb are fixed in the body frame. The velocity of 
the point in spatial coordinates is 

Vq a {t ) = ^Qa(t) = R ab (t)q b . (2.46) 

Thus R a b maps the body coordinates of a point to the spatial velocity 
of that point. This representation of the rotational velocity is somewhat 
inefficient, since it requires nine numbers to describe the velocity of a 
rotating body. One may use the special structure in the matrix R ab to 
derive a more compact representation. To this end, we rewrite equa¬ 
tion (2.46) as 

v qa (t) — Rab(t)R ab (t)R ab (t)q b . (2.47) 

The following lemma shows that R ab (t)R~ b (t) £ so( 3); i.e., it is skew- 
symmetric. 

Lemma 2.12. Given R(t) £ 50(3), the matrices i?(t)l? _1 (t) £ R 3x3 
and R~ l {t)R(t) £ R 3x3 are skew-symmetric. 

1 The word “spatial” is sometimes used to differentiate between planar motions in 

R 2 and general (spatial) motions in R 3 . In this chapter we reserve the word spatial to 

mean “relative to a fixed (inertial) coordinate frame.” 
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Proof. Differentiating the identity 


R(t)R{t) T = I 

we have, dropping the dependence of the matrices on t, 

RR t + RR T = 0, 

so that 

RR t = -(RR t ) t . 

Hence, RR~ X = RR T is a skew-symmetric matrix. The proof that R~ 1 R 
is skew-symmetric follows by differentiating the identity R T R = I. □ 

Lemma 2.12 allows us to represent the velocity of a rotating body 
using a 3-vector. We define the instantaneous spatial angular velocity, 
denoted u s ab € R 3 , as 

Q s ab :=R ab R~ b \ (2.48) 

The vector to s ab corresponds to the instantaneous angular velocity of the 
object as seen from the spatial (A) coordinate frame. Similarly, we define 
the instantaneous body angular velocity , denoted u> b b £ R 3 , as 

2ab ■■= R^Rab- ( 2 - 49 ) 

The body angular velocity describes the angular velocity as viewed from 
the instantaneous body ( B ) coordinate frame. From these two equations, 
it follows that the relationship between the two angular velocities is 

w ah — Rab^abRab Or = R ab U) S ab . (2.50) 

Thus the body angular velocity can be determined from the spatial angu¬ 
lar velocity by rotating the angular velocity vector into the instantaneous 
body frame. 

Returning now to equation (2.47), we can express the velocity of a 
point in terms of the instantaneous angular velocity of the rigid body. 
Substituting equation (2.48) into equation (2.47), 

v qa (*) = uJ s ab Rab(t)q b = u} s ab (t) x q a (t). (2.51) 

Alternatively, using equation (2.50), the velocity of the point in body 
frame is given by 


v qb (t) := Rl b {t)v qa it) = u b ab (t) X q b . (2.52) 

Equations (2.51) and (2.52) constitute a compact description of the ve¬ 
locity of all particles of the body in terms of the body and spatial angular 
velocities, u b ab and to s ab . 
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Figure 2.10: Rotational motion of a one degree of freedom manipulator. 


Example 2.4. Rotational motion of a one degree of freedom 
manipulator 

Consider the motion of the one degree of freedom manipulator shown 
in Figure 2.10. Let 0(f) be the angle of rotation about some reference 
configuration. The trajectory of the manipulator is given by 


m = 


Q s = RR = 


hence, 


—0sin0 
9 cos 9 
0 


cos 0 (f) —sin 0 (f) 0 

sin 0 (f) cos 0 (f) 0 

0 0 1 


The spatial velocity is 


—0cos0 0 
-0 sin 9 0 

0 0 


u> = 


cos 9 sin 9 0 

— sin 9 cos 9 0 

0 0 1 



'0 

-6 

o' 

= 

0 

0 

0 


_0 

0 

0_ 


The body velocity is 


U) b = r t r = 

1 

O 

-0 

0 

1 

o o 

or 

uj b = 

1 

O O ■ 

1_ 


0 

0 

0 



0 


4.2 Rigid body velocity 

Let us now consider the general case where g a b(t) £ SE( 3) is a one- 
parameter curve (parameterized by time) representing a trajectory of 
a rigid body: more specifically, the rigid body motion of the frame B 
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attached to the body, relative to a fixed or inertial frame A. As in the 
case of rotation, g a b(t) by itself is not particularly useful, but the two 
terms g a b9ab an d 9ab 9°-b have some special significance. With 


g a b(t) 


Rabiff Pabif') 

0 1 


we have that 


9abg a b 


Rab Pab 

^ab R a bPab 


RabR a i) RabRdbPab H - Pab 

0 0 

0 1 


0 0 


which has the form of a twist. By analogy to the rotational velocity, we 
define the spatial velocity Vff b £ se(3) as 


^ob — 9ab9 ab 


v lb 


RabR a i)Pab Pab 

f°ab. 


( RabR T ah Y 


(2.53) 


The spatial velocity Vff b can be used to find the velocity of a point in 
spatial coordinates. The coordinates of a point q attached to the rigid 
body in spatial coordinates are given by 

Qa(t) = g a b(t)q b . 


Differentiating yields 


^ q a — Qa — QabQb 


9abg ab 


9a 


and thus, 

v qa = V° b q a = u> s ab x q a T v s ab . (2.54) 

The interpretation of the components of the spatial velocity of a rigid 
motion is somewhat unintuitive. The angular component, to s ab , is the in¬ 
stantaneous angular velocity of the body as viewed in the spatial frame. 
The linear component, v s abl is not the velocity of the origin of the body 
frame , which is apparent from equation (2.53). Rather, v* b (t) is the ve¬ 
locity of a (possibly imaginary) point on the rigid body which is traveling 
through the origin of the spatial frame at time t. That is, if one stands at 
the origin of the spatial frame and measures the instantaneous velocity 
of a point attached to the rigid body and traveling through the origin at 
that instant, this is v^ b (t). 

A somewhat more natural interpretation of the spatial velocity is ob¬ 
tained by using the relationship between twists and screws described in 
the previous section. The screw associated with the twist Vf h gives the 
instantaneous axis, pitch, and magnitude of the rigid motion relative to 
the spatial frame. 
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It is also possible to specify the velocity of a rigid body with respect 
to the (instantaneous) body frame. We define 


^ab 9ab 9 ah 


^ab-Rab 

R'abPab 

II 

1 

-o 
■o C3 

5> 

i_ 


R ab Pab 

0 

0 

-o' 
-o e 

3 

_ i 


_{R T abRab)\ 


(2.55) 

to be the body velocity of a rigid motion g ab (t) £ SE( 3). The velocity of 
the point in the body frame is given by 

V Qb = 9abV qa = g~£gabqb = Va b (t)q b - 

Thus, the action of V b b is to take the body coordinates of a point, q b , and 
return the velocity of that point written in body coordinates, v qb : 

Vq b = V b b q b = u b ab xq b + v b ab . (2.56) 

The interpretation of the body velocity is straightforward: v b b is the 
velocity of the origin of the body coordinate frame relative to the spatial 
frame, as viewed in the current body frame. u> b b is the angular velocity 
of the coordinate frame, also as viewed in the current body frame. Note 
that the body velocity is not the velocity of the body relative to the body 
frame; this latter quantity is always zero. 

The spatial and body velocity of a rigid motion are related by a sim¬ 
ilarity transformation. To calculate this relationship, we note that 

Vab = 9abg ab = Safc(<7 0 6 9ab)g ab = gab V ab 9 ab ' 

Alternatively, we can write 

^ab -^ab^ab 

Vab = w ab X Pab T Pab = Pab x (^ab^ab) T RabV ab - 


In either case, we may summarize the calculation as 


V a s b = 


V ab 


Rab 

Pab^ab 

_ U lb. 


0 

Rab 


(2.57) 


The 6x6 matrix which transforms twists from one coordinate frame to 
another is referred to as the adjoint transformation associated with g, 
written Ad ff . Thus, given g € SE( 3) which maps one coordinate system 
into another, Ad g : R 6 —> R 6 is given as 


Adg 


R pR 
0 R 


(2.58) 
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In the calculation that we have just performed, Ad ff maps body velocity 
twist coordinates to spatial velocity twist coordinates. Ad ff is invertible, 
and its inverse is given by 


Ad^ 1 = 


R t -( R T p) A R 7 

0 r t 



■ r t 

- R t p 


0 

r t 


= M ?- 1 


(see Exercise 14). 

We shall make frequent use of the adjoint transformations throughout 
the book. The calculations performed above give the following useful 
characterization of the adjoint associated with a rigid transformation g £ 
SE( 3): 

Lemma 2.13. Iff £ se(3) is a twist with twist coordinates f £ R 6 , then 
for any g £ SE( 3), gfg -1 is a twist with twist coordinates Ad ff £ £ R 6 . 

It will often be convenient to define velocity without explicit reference 
to coordinate frames. For a rigid body with configuration g £ SE(3), we 
define the spatial velocity as 


= gg - 1 

and the body velocity as 
V b = g~ l g 


F s = 


V b = 


V 


— RR T p + p 

UJ S 


{rr t ) w 


'v b ' 


R T p 

. ujb . 


(. R t R) w 


(2.59) 


(2.60) 


The body and spatial velocities are related by the adjoint transformation, 

V s = Ad s V b . (2.61) 


Example 2.5. One degree of freedom manipulator 

Consider the one degree of freedom manipulator shown in Figure 2.11. 
The configuration of the coordinate frame B relative to the fixed frame 
A is given by 

— sind(t) 0 —12 sin 9(t) 

cos 9(t) 0 l\ + l 2 COs 6 (t) 

0 1 /o’ 

0 0 1 

where we drop all subscripts for simplicity. The spatial velocity of the 
rotating rigid body is given by 

v s = — RR T p + p 
u s = {RR T y. 



g(t) = 


cos 6 {t) 
sin 0(f) 
0 
0 
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Figure 2.11: Rigid body motion generated by rotation about a fixed axis. 


Using the calculation of uj s from the previous example, we have 



\0 


'O' 

II 

CO 

0 

II 

CO 

3 

O 


l 

o 


_e_ 


Note that V s is precisely the velocity of a point attached to the rigid body 
as it travels through the origin of the A coordinate frame. 

The body velocity is 

v b = R T p 

tu b = (R t R)' / , 

which gives 

O' 

0 . 

e_ 

The body velocity can be interpreted by imagining the velocity of the 
origin of the B coordinate frame, as seen in the B coordinates. Thus, the 
linear velocity is always in the —x direction and the angular velocity is 
always in the z direction. The magnitude of the linear component of the 
velocity is dependent on the length of the link connecting the B frame to 
the joint. 

4.3 Velocity of a screw motion 

In the previous example, we calculated the spatial velocity of a rigid mo¬ 
tion generated by a screw action, exp(£0). Referring back to Example 2.3 


v b = 


-he 

0 

0 
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in the previous section, we see that the spatial velocity V s in the example 
above is identical to £ when 6 = 1. Consider the more general case where 

9ab{0) = e&gabi 0 ) 

represents the configuration of coordinate frame B relative to frame A. 
Using the fact that for a constant twist £, 

(see Exercise 8), the spatial velocity for this rigid body motion is 

V: b = 9ab(0)g- b \6 ) 

= (W e <?a&(0)) ( 5 - b 1 (0)e-« e ) 

= $• 

Thus, the spatial velocity corresponding to this motion is precisely the 
velocity generated by the screw. 

The body velocity of a screw motion can be calculated in a similar 
manner: 

V b ab = 9ab\0)g ab {e) 

= (dab^e-V) (Jde&gabi 0 )) 

= (^(0)^(0)) 6 = (Ad g -i (0) ^ 6. 

For 6 = 1, V^ b is a constant vector in the moving body frame. The di¬ 
rection of the body velocity twist is given by the adjoint transformation 
generated by the initial configuration of the rigid body, g~ b { 0). In par¬ 
ticular, if g ab { 0) = /, i.e., the body frame and spatial frame coincide at 
6 = 0 , then V° b = V£ b = £ 6 , where ^ is the constant twist which generates 
the screw motion. 

4.4 Coordinate transformations 

Just as we can compose rigid body transformations to find g ac £ SE( 3) 
given g ab ,g b c £ S!E(3), it is possible to determine the velocity of one 
coordinate frame relative to a third given the relative velocities between 
the first and second and second and third coordinate frames. We state 
the main results as a set of propositions. 

Proposition 2.14. Transformation of spatial velocities 

Consider the motion of three coordinate frames , A, B, and C. The fol¬ 
lowing relation exists between their spatial velocities: 

V° c = V a s b + Ad gab V b s c . 
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Proof. The configuration of frame C relative to A is given by 


9ac 9ab9bc- 


By definition and the chain rule, 


^ac 9ac9ac 

( 9ab9bc A 9ab9bc)(9bc 9 a b ) 
= 9ab9 a b A 9ab{9bc9 bc )9 a b 
= V: b Ag ab V b s c g- b \ 


and converting to twist coordinates, 



□ 


Proposition 2.15. Transformation of body velocities 

Consider motion of three coordinate frames, A, B, and C. The following 
relation exists between their relative body velocities: 



Proof. Application of the chain rule, as above. 


□ 


Propositions 2.14 and 2.15 are used to transform the velocity of a rigid 
body between different coordinate frames. Often, two of the coordinate 
frames are stationary with respect to each other and the velocity rela¬ 
tionships can be simplified. As an example, if A and B are two inertial 
frames which are fixed relative to each other, then the spatial velocity of 
a frame C satisfies 


(2.62) 


K s c = Ad Sab 

The corresponding relationship for body velocities is 



(2.63) 


since the body velocity is independent of the inertial frame with respect 
to which it is measured. 

The transformation rules given by Propositions 2.14 and 2.15 can also 
be applied to constant twists, such as those used to model revolute and 
prismatic joints. If £ is a twist which represents the motion of a screw 
and we move the screw by applying a rigid body motion g £ SE( 3), the 
new twist can be obtained using equation (2.62). We interpret g as a 
fixed rigid motion and equate £ with a spatial velocity vector. In this 
case, g = 0 and hence 


£' = Ad s £ or £ =g£g 1 . 


(2.64) 
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Figure 2.12: Two degree of freedom manipulator. 


This formula is of tremendous importance in the chapters to come, where 
we will need to keep track of the different twist axes corresponding to the 
joints of a robot when they are moved. 


Example 2.6. Velocity of a two-link mechanism 

Consider the two degree of freedom manipulator shown in Figure 2.12. 
We wish to find the velocity of frame C relative to A, given the joint 
velocities 6\, 02 £ M. Since each motion is a screw motion, we write 


/ ab 


v bc 





'O' 


'o' 

= 

Vab 

01 Vab = 

0 

^ab — 

0 


(Vab 


0 


1 




'll 


'o' 

= 

Vbc 

II 

u 

-0 

(N 

0 

^bc = 

0 


(Vbe 


0 


1 


We also calculate Ad Sab : 




Rab 

0 


/ 0 \ A „ ' 

U) R “ b 

Rab 


Using Proposition 2.14, 


C = v a s b 


Ad flafc V b s c 


"O' 


■ / 1 COS 01 - 

0 


1 1 sin 6\ 

0 

0 

0i + 

0 

0 

0 


0 

_ 1 _ 


L 1 J 


Note that the velocity consists of two components, one from each of the 
joints, and that they add together linearly. 
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A few other identities between body and spatial velocities will prove 
useful in subsequent chapters. We give them here in the form of a lemma. 
Their proof is left as an exercise. 

Lemma 2.16. Rigid body velocity identities 

Using the notation given above for the velocity of one coordinate frame 
relative to another, the following relationships hold: 

V b ab = -v b s a 
V b b = -Ad gha V b b a . 

5 Wrenches and Reciprocal Screws 

In this section we consider forces and moments acting on rigid bodies and 
use this to introduce the notion of screw systems and reciprocal screws. 

5.1 Wrenches 

A generalized force acting on a rigid body consists of a linear component 
(pure force) and an angular component (pure moment) acting at a point. 
We can represent this generalized force as a vector in R 6 : 

/ € R 3 linear component 
r S R 3 rotational component 

We will refer to a force/moment pair as a wrench. 

The values of the wrench vector F £ R 6 depend on the coordinate 
frame in which the force and moment are represented. If B is a coordinate 
frame attached to a rigid body, then we write F b = (f b ,T b ) for a wrench 
applied at the origin of B , with f b and r b specified with respect to the B 
coordinate frame. 

Wrenches combine naturally with twists to define instantaneous work. 
Consider the motion of a rigid body parameterized by g ab (t), where A 
is an inertial frame and B is a frame attached to the rigid body. Let 
V b b £ R 6 represent the instantaneous body velocity of the rigid body 
and let F b represent an applied wrench. Both of these quantities are 
represented relative to the B coordinate frame and their dot product is 
the infinitesimal work: 



SW = V b b -F b = (v- f +uj- t). 

The net work generated by applying the wrench F b through a twist V b b 
over a time interval [ti, ^ 2 ] is given by 

w= r v b ab -F b dt. 

.It 1 
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Figure 2.13: Transformation of wrenches between coordinate frames. 


Two wrenches are said to be equivalent if they generate the same work 
for every possible rigid body motion. Equivalent wrenches can be used to 
rewrite a given wrench in terms of a wrench applied at a different point 
(and with respect to a different coordinate frame). An example of this is 
shown in Figure 2.13: given the wrench F b applied at the origin of contact 
coordinate frame B, we wish to determine the equivalent wrench applied 
at the origin of the object coordinate frame C. In order to compute 
the equivalent wrench, we use the instantaneous work performed by the 
wrench as the body undergoes an arbitrary rigid motion. Let gb c = 
( pbc, Rbc ) be the configuration of frame C relative to B. By equating the 
instantaneous work done by the wrench Fb and the wrench F c over an 
arbitrary interval of time, we have that 

Vl ■ F c = V b b ■ F b = (Ad gbc V a b c ) T F b = V b c ■ Adl c F b , 
and since V b c is free, 

F c = AdJ bc F b . (2.65) 

Equation (2.65) transforms a wrench applied at the origin of the B frame 
into an equivalent wrench applied at the origin of the C frame. The com¬ 
ponents of F c are specified relative to the C coordinate frame. Expanding 
equation (2.65), 


7c' 


c 

0 ' 

'fb 

Jc 


R-bcPbc 


Jb_ 


we see that the adjoint transformation rotates the force and torque vectors 
from the B frame into the C frame and includes an additional torque of 
the form —pb c x fb, which is the torque generated by applying a force fb 
at a distance —pb c - 

It is also possible to represent a wrench with respect to a coordinate 
frame which is not inside the rigid body. Consider for example the co- 


62 















ordinate frame A shown in Figure 2.13. The wrench F written in A’s 
coordinate frame is given by 


Fa = AdJ ba F b . 

This wrench represents the equivalent force/moment pair applied as if 
the coordinate frame A were rigidly attached to the object. This is not 
the same as simply rewriting the components of Ff, in A’s coordinates, 
since the point of application for F a is the origin of the A frame and not 
the origin of the B frame. 

If several wrenches are all applied to a single rigid body, then the net 
wrench acting on the rigid body can be constructed by adding the wrench 
vectors. In order for this addition to make sense, all of the wrenches 
must be represented with respect to the same frame. Thus, given a 
set of wrenches Fi, each wrench is first written as an equivalent wrench 
relative to a single coordinate frame and then the equivalent wrenches 
are added to determine the net wrench acting on the rigid body. This 
helps explain why equivalent wrenches include a shift of origin: one can 
only add wrenches if they represent forces and torques applied at a single 
point (such as the center of mass or a fixed spatial frame). 

A net wrench F acting on a rigid body with configuration g a {, £ 
SE( 3) has two natural representations. The body representation of the 
wrench is written as Fb and represents the equivalent force and moment 
applied at the origin of the B frame (and written in B 's coordinates). The 
spatial representation of the wrench is the equivalent wrench written in 
A’s coordinate frame. These representations are analogous to the spatial 
and body representations of the velocity of a rigid body. 

As with velocities, it will be convenient to define the spatial and body 
representations of a wrench without explicit reference to a given set of 
coordinate frames. If g £ SE(3) is the configuration of a rigid body, then 
we write F b for the body wrench and F s for the spatial wrench. These 
wrenches are related by the transpose of the adjoint matrix: 

F b = AdJ F s . (2.67) 

This notation mirrors that used for body and spatial velocities of a rigid 
body allowing the instantaneous work performed by a wrench F moving 
through a rigid motion with instantaneous velocity V to be written as 

SW = V b ■ F b = V S ■ F s . 

We leave the proof of this statement as an exercise. 

Example 2.7. Preview of multifingered grasping 

Consider the multifingered grasp shown in Figure 2.14. Let F Ci be the 
wrench exerted by the ith finger on the grasped object, represented in 
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Figure 2.14: Coordinate frames for a simple grasping example. 

the frame C*. The net wrench on the body, in the body coordinate frame 
O, is given by 



This is the basic calculation which is used in grasping to determine the 
net effect of forces applied at the contact points between the fingers and 
the object. 

5.2 Screw coordinates for a wrench 

As with twists, it is possible to generate a wrench by applying a force 
along an axis in space and simultaneously applying a torque about the 
same axis. The dual of Chasles’ theorem, which showed that every twist 
could be generated by a screw, is called Poinsot’s theorem. It asserts that 
every wrench is equivalent to a force and torque applied along the same 
axis. We begin by defining the notion of a wrench acting along a screw. 

With respect to some fixed spatial coordinate frame A, let S' be a screw 
with axis l = {q + Xui : A G R}, ||u;|| = 1, pitch h, and magnitude M. We 
construct a wrench from this screw by applying a force of magnitude M 
along the directed line l and a torque of magnitude hM about the line. 
If h = oo, we generate a wrench by applying a pure torque about l. The 
resulting wrench, in A’s coordinates, is given by 



h finite 


h = oo, 


( 2 . 68 ) 
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where the term — u> x q accounts for the offset between the axis of the 
screw and the origin of A. We call F the wrench along the screw S. Note 
that F (and q and u>) are all specified with respect to the fixed coordinate 
frame A and hence F represents the spatial wrench applied to the rigid 
body. (We omit the use of subscripts in this section since all quantities 
are specified with respect to a single coordinate frame.) 

To find the screw coordinates for a wrench, we solve equation (2.68) 
for u>, q , h , and M given F = (/, r). This leads to the following theorem: 

Theorem 2.17 (Poinsot). Every collection of wrenches applied to a rigid 
body is equivalent to a force applied along a fixed axis plus a torque about 
the same axis. 

Proof. The proof is constructive. Let F = (/, r) be the net wrench 
applied to the object. We ignore the trivial case, F = 0. 

Case 1: (f = 0, pure torque). Set M = ||r||, u> = t/M , and h = oo. 
Equation (2.68) verifies that these are the appropriate screw coordinates. 

Case 2: (f ^ 0). Set M = ||/||, and w = f/M. It remains to solve 

M(q x uj + hut) = r 
for q and h. One solution is given by 



This solution is not unique since any q' = q + A ui will also satisfy equa¬ 
tion (2.68). .jj 

Using Poinsot’s theorem, we can define the screw coordinates of a 
wrench, F = (/, r): 


1. Pitch: 



(2.69) 


The pitch of a wrench is the ratio of angular torque to linear force. 
If / = 0, we say that F has infinite pitch. 


2. Axis: 


l = 


{0 + At : A G R}, 


if/ 7^0 
if / = 0 


(2.70) 


The axis l is a directed line through a point. For / ^ 0, the axis 
is a line in the / direction going through the point q = For 

/ = 0, the axis is a line in the r direction going through the origin. 
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3. Magnitude: 


M = 


ll/ll, */7^0 
IMI> if / = 0 


(2.71) 


The magnitude of a screw is the net linear force, if the motion 
contains a linear component, or the net torque, otherwise. 

The dual nature of twists and wrenches is evident in the screw coordinates 
for twists and wrenches. For example, a zero pitch twist corresponds to 
pure rotation, while a zero pitch wrench corresponds to a pure force (no 
angular component). 

5.3 Reciprocal screws 

The dot product between twists and wrenches gives the instantaneous 
power associated with moving a rigid body through an applied force. As 
in the previous subsection, we carry out all calculations relative to a single 
coordinate frame and omit the use of subscripts. A wrench F is said to 
be reciprocal to a twist V if the instantaneous power is zero: F ■ V = 0. 
Since both twists and wrenches can be represented by screws, we can use 
this to define the notion of reciprocal screws: 

Definition 2.3. Reciprocal screws 

Two screws Si and S 2 are reciprocal if the twist V about Si and the 
wrench F along S 2 are reciprocal. 

Classically, reciprocal screws are defined by using the reciprocal prod¬ 
uct between screws. Let S,; be a screw with axis li = {qi + A u>i : A £ M}, 
pitch hi , and magnitude Mi. Given two screws S\ and S 2 , we define the 
distance d between the screws as the minimum distance between l\ and 
I 2 ', this distance will be achieved along a line perpendicular to both l\ 
and I 2 • We denote this line as dn where n is a unit vector and d > 0. 
The angle a between S\ and S 2 is the angle between the vectors u>i and 

a = atan2(wi x 102 ■ n, uj\ ■ 0 J 2 ) 

(see Figure 2.15). The reciprocal product between two screws is defined 
as 

Si © S 2 = Mi M 2 ((hi + / 12 ) cos o: — dsina). (2.72) 

Proposition 2.18. Characterization of reciprocal screws 

Two screws S 1 and S 2 are reciprocal if and only if 


Si 0 S 2 — 0. 


Proof. We consider only the case where hi and /12 are finite. The other 
cases are left as exercises. Let V be the twist about the screw Si and F 
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Figure 2.15: Notation for reciprocal screws. 


be the wrench along the screw S 2 '- 


V = Mi 


Q\ X LUi T 

UJl 


F = M 2 


oj 2 

q 2 x bj 2 + h 2 u) 2 


Without loss of generality we can assume that q\ and q 2 are the points 
at which the axes are closest and hence q 2 can be rewritten as q 2 = 
qi + dn where n is the unit normal vector connecting the two axes. The 
instantaneous work between V and F is 

V ■ F = M\M 2 (oJ 2 ' (9l h\U)\) + id 1 ■ (q 2 XW 2 + / 12 W 2 )) 

= M 1 M 2 (id 2 ' q\ x uj\ h\id\ * &1 ' (qi T du) x uj 2 T h 2 ^i * ^ 2 ) 

= M\M 2 ((hi + h 2 ) cos a — d sin a) , 

which is precisely the reciprocal product. Hence, by definition, the screws 
are reciprocal if and only if the reciprocal product is zero. Q 

If we represent screws using twist coordinates, then we can define 
the reciprocal product directly in terms of the components of the twists. 
Let Vl,V 2 G R 6 be two arbitrary twists. Then we define the reciprocal 
product between V\ and V 2 as 


V 1 QV 2 — vj’id 2 + 


A similar relationship holds if we associate screws with wrenches. 

Reciprocal screws play an important role in analyzing the kinematic 
properties of mechanisms. For example, in a grasping context we can 
view the wrenches applied to an object as a set of constraining screws 
and ask if there are any instantaneous rigid motions (twists) that do not 
violate the constraints. Such twists, if they exist, correspond to motions 
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Figure 2.16: A set of pure forces acting on a rigid body. 

of the grasped object which cannot be restricted by the fingers. This 
specific situation is considered in detail in Chapter 5, but we can give 
some preliminary indications of how the analysis might proceed using 
the concept of a system of screws. 

As a motivating example, consider the grasping situation depicted in 
Figure 2.16. Suppose we constrain the motion of a rigid body by ap¬ 
plying normal forces at several points around the rigid body. We would 
like to ascertain if there are any motions of the rigid body which cannot 
be resisted by these forces. Let {Si,..., Sk} represent the screws corre¬ 
sponding to the wrenches. Suppose that there exists another screw Sf 
such that Sf Q Si = 0. Then, interpreting S/ as a twist and each Si as 
a wrench, we see that motion along Sf causes no work to be performed 
against any of the wrenches. Hence, the wrenches cannot resist this type 
of motion and the object is free to move (instantaneously) along Sf. 

If we interpret a set of screws {Si,..., Sk} as twists, then the twists 
form a linear space over the reals and hence we can talk about scaling 
and adding screws by interpreting this in terms of regular addition and 
multiplication on twists. We call the set of screws {Si,..., Sk} a system 
of screws and we define addition and scaling of screws by associating each 
screw with a unique twist. 

It follows immediately from the definition of the reciprocal product 
that if S is reciprocal to Si and S 2 , then it is reciprocal to any linear 
combination of Si and S 2 (with the linear combination performed in twist 
coordinates). Using this linearity property, we can define the set of all 
screws which are reciprocal to a given system of screws as the recipro- 
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cal screw system. A reciprocal screw system defines a linear subspace of 
twists. If we interpret a screw system as a set of wrenches (or constrained 
directions), then the reciprocal screw system describes the instantaneous 
motions which are possible under the constraints. Conversely, if we inter¬ 
pret the screw system as a set of twists, then the reciprocal screw system 
is the set of wrenches which cause no net motion of the object. Both of 
these interpretations follow directly from the definition of the reciprocal 
product between a twist and a wrench. 

In addition to applications in grasping, screw systems and reciprocal 
screw systems can be also used to analyze the mobility of mechanisms, 
as we shall see in detail in the next chapter. The following proposition is 
one of the main tools in this type of analysis. Its proof follows directly 
from the fact that the space of twists is a 6-dimensional linear space and 
that screws can be naturally associated with this linear space. 

Proposition 2.19. Dimensionality of reciprocal screw systems 

Let r be the dimension of system of screws {S±,..., Sk} (determined by 
converting the screws into either twists or wrenches) and let n be the 
dimension of the corresponding reciprocal system. Then, 

r + n = 6. 

Applying this proposition to the example in Figure 2.16, we see that 
the subspace of twists which cannot be resisted is at least 3-dimensional. 
It may have greater dimension if the applied normal forces do not generate 
independent wrenches. 
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6 Summary 

The following are the key concepts covered in this chapter: 

1. The configuration of a rigid body is represented as an element g £ 
SE( 3). An element g £ SE(3) may also be viewed as a mapping 
g : M 3 —> R 3 which preserves distances and angles between points. 
In homogeneous coordinates, we write 


9 = 


R p 
0 1 


R £ SO{ 3) 
p £ R 3 . 


The same representation can also be used for a rigid body transfor¬ 
mation between two coordinate frames. 

2. Rigid body transformations can be represented as the exponentials 
of twists: 


g = exp(£0) £ = 


0 0 


LJ £ so(3), 
v £ R 3 ,6> € 


The twist coordinates of £ are £ = (v, u) £ R 6 . 

3. A twist £ = (v,u>) is associated with a screw motion having at¬ 
tributes 


pitch: 


h = 


T 

LO V 


axis: 


magnitude: 


l = 


M = 


1PF + XuJ: A e 

{0 T Xv : A £ R}, 

INI, ifw^O 
Hull, if = 0. 


if lu y o 

if u> = 0; 


Conversely, given a screw we can write the associated twist. Two 
special cases are pure rotation about an axis l = {q + Aiu} by an 
amount 6 and pure translation along an axis l = {0 + Xv}: 


£ = 


— u> x q 

UJ 


9 (pure rotation) £ = 


9 (pure translation). 


4. The velocity of a rigid motion g(t) £ SE(3) can be specified in two 
ways. The spatial velocity , 


= 99 
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is a twist which gives the velocity of the rigid body as measured by 
an observer at the origin of the reference frame. The body velocity , 

V b = g-'g, 


is the velocity of the object in the instantaneous body frame. These 
velocities are related by the adjoint transformation 

F s = Ad g V b Adg = 

which maps M 6 —> K 6 . To transform velocities between coordinate 
frames, we use the relations 

V a s c = V° b + Ad gab V b s c 

Vac = Ad g -i Vab + ^6c) 

where V* b is the spatial velocity of coordinate frame B relative to 
frame A and V b b is the body velocity. 

5. Wrenches are represented as a force, moment pair 

A=(/,t)£R 6 . 

If B is a coordinate frame attached to a rigid body, then we write 
Fb = ( fb , Tb) for a wrench applied at the origin of B , with fb and 
specified with respect to the B coordinate frame. If C is a second 
coordinate frame, then we can write Fb as an equivalent wrench 
applied at C: 

F c =Ad T gbc F b . 

For a rigid body with configuration g a b , F s := F a is called the 
spatial wrench and F b := Fb is called the body wrench. 

6. A wrench F = (/, r) is associated with a screw having attributes 


pitch: 

/ fTr 
n/ir 

axis: 

flfw + V-- ^1}, if/^o 


|{0 + At : A £ R}, if / = 0 


f II fII, if f A 0 

magnitude: 

M=< J ^ 


IJMli if / = 0 . 


R pR 
0 R ’ 


7. A wrench F and a twist V are reciprocal if F ■ V = 0. Two screws 
S i and S 2 are reciprocal if the twist Vj about Si and the wrench F 2 
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along *5*2 are reciprocal. The reciprocal product between two screws 
is given by 


Si0S 2 — V 1 -F 2 — Vi 0 V 2 — vi ■ uj 2 i>2 u}i 


where V) = ( Vi,Ui ) represents the twist associated with the screw 
Si. Two screws are reciprocal if the reciprocal product between the 
screws is zero. 

8. A system of screws {Si,..., Sk} describes the vector space of all 
linear combinations of the screws {Si,..., Sk}- A reciprocal screw 
system is the set of all screws which are reciprocal to Si- The 
dimensions of a screw system and its reciprocal system sum to 6 
(in SE{ 3)). 

All of the concepts presented in this chapter can also be applied to planar 
rigid body motions (see Exercises 10 and 11). 
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The treatment of rigid motion described here, particularly the geometry 
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les [1], and McCarthy [70]. A more abstract version of the developments 
of this chapter can be made in the framework of matrix Lie groups and 
is presented in Appendix A. 
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8 Exercises 

1. Let a, 6, c G R 3 be 3-vectors and let • and x denote the dot product 
and cross product in R 3 . Verify the following identities: 

(a) a ■ (b x c) = (a x b) ■ c 

(b) a x (b x c) = (a - c)b — (a • b)c 

2. Using the homogeneous representation, show that SE(3) satisfies 
the axioms of a group, with the group multiplication given by the 
usual matrix multiplication. 

3. Properties of rotation matrices 

Let R £ 50(3) be a rotation matrix generated by rotating about a 
unit vector uj by 6 radians. That is, R satisfies R = exp(u;0). 

(a) Show that the eigenvalues of u> are 0, *, and — i, where i = 
yf—\. What are the corresponding eigenvectors? 

(b) Show that the eigenvalues of R are 1, e l9 , and e~ l6 . What is 
the eigenvector whose eigenvalue is 1? 

(c) Let R = [ri r 2 r 3 ] be a rotation matrix. Show that det R = 
rf (r 2 x r 3 ). 

4. Properties of skew-symmetric matrices 

Show that the following properties of skew-symmetric matrices are 
true: 

(a) If R £ 50(3) and ui £ R 3 , then RujR t = (i?w) A . 

(b) If R € 50(3) and v, w € R 3 , then R(v x w) = (Rv) x (Rw ). 

(c) Show that so(3) is a vector space. Determine its dimension 
and give a basis for so(3). 

5. Cayley parameters 

Another parameterization of 50(3), which does not involve tran¬ 
scendental functions, is Cayley’s parameterization. Let a be a vector 
in R 3 and let a be the associated 3x3 skew-symmetric matrix. 

(a) Show that R a = (I — a) -1 (I + a) € 50(3). 

(b) Verify that 


1+aj — a|— a§ 2(aia 2 — a 3 ) 2(aia 3 + a 2 ) 
2(aia 2 +a 3 ) l — al+a^-al 2(a 2 a 3 - an) 
2(aia 3 — a 2 ) 2(a 2 a 3 + m) 1 — af— a|+a| 



(c) Given a rotation matrix R, compute the Cayley parameters a. 


73 





6. Unit quaternions 

Let Q = (qo,q) and P = (po,p) be quaternions, where qo,Po £ R 
are the scalar parts of Q and P and q,p are the vector parts. 

(a) Show that the set of unit quaternions satisfies the axioms of a 
group. 

(b) Let a; be a point and let X be a quaternion whose scalar part is 
zero and whose vector part is equal to x (such a quaternion is 
called a pure quaternion). Show that if Q is a unit quaternion, 
the product QXQ* is a pure quaternion and the vector part 
of QXQ* satisfies 

(<7o - q- q)x + z(qo{q xx) + (x- q)qj. 

Verify that the vector part describes the point to which x is 
rotated under the rotation associated with Q. 

(c) Show that the set of unit quaternions is a two-to-one covering 
of 50(3). That is, for each R £ 50(3), there exist two distinct 
unit quaternions which can be used to represent this rotation. 

(d) Compare the number of additions and multiplications needed 
to perform the following operations: 

i. Compose two rotation matrices. 

ii. Compose two quaternions. 

iii. Apply a rotation matrix to a vector. 

iv. Apply a quaternion to a vector [as in part (b)]. 

Count a subtraction as an addition, and a division as a multi¬ 
plication. 

(e) Show that a rigid body rotating at unit velocity about a unit 
vector in w £ R 3 can be represented by the quaternion differ¬ 
ential equations 

Q-Q* = ( 0,w/2), 

where • represents quaternion multiplication. 

7. A rigid body moving in R 2 has three degrees of freedom (two com¬ 
ponents of translation and one of rotation), a rigid body moving in 
R 3 has six degrees of freedom (three each of translation and rota¬ 
tion). Show that a rigid body moving in R" will have |(n + n 2 ) 
degrees of freedom. How many are translational and how many are 
rotational? 


8. Properties of the matrix exponential 

Let A be a matrix in R raxn . The exponential of A is defined as 


e 


A 


1 + A + 


A 2 

2 ! 


A 3 

3! 


H- 
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(a) Choose a matrix norm and show that the above series con¬ 
verges. 

(b) Let g £ R raxra be an invertible matrix. Show the following 
equality: 

A —1 oAo 1 
ge g = e u y . 

(c) Verify that 

j t e Ke = (A 9)e Ae = e Ae (A9). 

9. Projection maps and proof of Proposition 2.9 

This problem completes the proof of Proposition 2.9 using the prop¬ 
erties of projection maps on linear spaces. Assume w £ so(3) and 

IMl = i. 

(a) Given a vector co £ R 3 , let denote the subspace spanned 
by to and N A denote the orthogonal complement. Show that 

image u; = N A and kernel co = N u . 

(b) Let V C R” be a linear subspace. A projection map is a linear 
mapping Py : R" —> V which satisfies image (Py) = V and 
Py(x) = x for all x £ V. Show that 

Pn^ = loco t and Pjy± = (/ — coco T ) 
are both projection maps. 

(c) Calculate the null space of I—e“ e for co £ so( 3) and 9 £ (0, 27t) 
and show that (/ — e u8 ) : N A —> N A is bijective. 

(d) Let A = (J — e u8 )co + coco T 9 1 where 9 £ (0, 27r). Show that 
A : R 3 —> R 3 is invertible. 


10. Planar rotational motion 

Let 50(2) be the set of all 2 x 2 orthogonal matrices with determi¬ 
nant equal to +1. 

(a) Show that 50(2) can be identified with the S 1 , the unit circle 
in R 2 . 

(b) Let co £ R be a real number and define u) £ so( 2) as the 
skew-symmetric matrix 

^ 0 — co 


Show that 


e ae 


cosu >9 — sinw# 
sintu0 cos to9 


Is the exponential map exp : so{ 2) —» 50(2) surjective? injec¬ 
tive? 
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(c) Show that for R £ SO( 2) and Q £ so( 2), Ru)R T = u. 

11. Planar rigid body transformations 

A transformation g = (p, i?) £ 5i£(2) consists of a translation p £ 
R 2 and a 2 x 2 rotation matrix R. We represent this in homogeneous 
coordinates as a 3 x 3 matrix: 


A twist £ £ se(2) can be represented by a 3 x 3 matrix of the form: 


cu 

V 


0 

—UJ 

0 

0 

cu = 

UJ 

0 


w £ R,t> £ R 2 . 


The twist coordinates for £ £ se(2) have the form £ = (v,w) £ R 3 . 
Note that v is a vector in the plane and w is a scalar. 


(a) Show that the exponential of a twist in se(2) gives a rigid body 
transformation in SE( 2). Consider both the pure translation 
case, £ = (u,0), and the general case, £ = (v,uj),uj ^ 0. 

(b) Show that the planar twists which correspond to pure rotation 
about a point q and pure translation in a direction v are given 

by 



q y 


v x 

z = 

Qx 

(pure rotation) £ = 

Vy 


1 


0 


(pure translation). 


(c) Show that every planar rigid body motion can be described 
as either pure rotation about a point (called the pole of the 
motion) or pure translation. 

(d) Show that the matrices V s = gg _1 and V b = g~ x g are both 
twists. Define and interpret the spatial velocity V s £ R 3 and 
the body velocity V b £ R 3 . 

(e) The adjoint transformation is used to map body velocities 
V b £ R 3 into spatial velocities V s £ R 3 . Show that the adjoint 
transformation for planar rigid motions is given by 


Ad g = 


R 

0 



12. Verify that for to £ R 3 , ||w|| ^ 1 

= i + jj^ sin (IMI 6 0 + yp^ 1 _ C0S (IMI 6 '))- 
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13. Let 5a = (— oj a x q a + hu> a -,u) a ) be the twist associated with a screw 
having pitch h and axis l = {q a + Xco a ■ A G K}, where all quantities 
are specified relative to a coordinate frame A. 

(a) Let B be a second coordinate frame with configuration g a {, G 
SE( 3). Show that the representation of the twist relative to B 
is given by 

= Ad fla6 £a = Ad S!m £a- 

(b) Suppose instead that we move the screw via a rigid body trans¬ 
formation g G SE{ 3). Show that the transformed screw can be 
represented by the twist 

£a = Adg 5a; 

still relative to the A coordinate frame. 

14. Use homogeneous representations to show that the following iden¬ 
tities hold: 

(a) (Adg)" 1 = Ad g -i for all g G SE{ 3). 

(b) Ad Slff2 = Ad Sl Ad ff2 for all 51,32 G SE{ 3). 

15. Prove Proposition 2.15: V^ c = Ad g -i V£ b + V bc . 

16. Figure 2.17 shows a two degree of freedom manipulator. Let Iq, I 1 J 2 
be the link length parameters and 6 \, 62 the joint angle variables of 
link 1 and link 2, respectively. 

(a) Express the position and orientation of frame C 3 relative to 
frame Cq in terms of the joint angle variables and the link 
parameters. 
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(b) Compute the spatial velocity of C 3 relative to Co as functions 
of the joint angles and the joint rates. 

(c) Compute the body velocity of C 3 relative to Co as functions 
of the joint angles and the joint rates. 

(d) Optional: Find the spatial velocity of the origin of C 3 and use 
this to check your answer for parts (b) and (c). 

You may want to use a symbolic math package, such as the one 
described in Appendix B, to carry out the computations in this 
exercise. 

17. Frame invariance and reciprocal screw systems 

An operator is said to be frame invariant if it does not depend on 
the choice of coordinate frame used to carry out the calculations. 
Operations which are frame invariant can be computed relative to 
any coordinate frame, which can simplify calculations. 

(a) Show that the reciprocal product between two screws is frame 
invariant. 

(b) Show that the inner product between two twists is not frame 
invariant. 

(c) Calculate a basis for the system of screws reciprocal to a zero- 
pitch screw through a point q. Give a geometric interpretation 
for the screws which form your basis. (Hint: perform your 
calculations relative to a specially chosen frame.) 

(d) Calculate a basis for the system of screws reciprocal to an 
infinite pitch screw. Give a geometric interpretation for the 
screws which form your basis. 

(e) Using reciprocal screws, show that three parallel, coplanar, 
zero-pitch screws are dependent. That is, exhibit a system of 
four independent screws which are reciprocal to each of the 
coplanar screws. 

18. Hybrid representation of velocity 

A seemingly natural way of representing the velocity of a rigid body 
is to use p to represent the linear velocity and oj s to represent the 
angular velocity. We call Vff b = (p,co s ) the hybrid velocity of a rigid 
body. 

(a) Show that the hybrid velocity is related to the body velocity 
by the relationship 


V h 


R 

0 


0 

R 


V b 
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and hence represents the velocity of the rigid body relative to 
a frame attached to the origin of the rigid body, but whose 
orientation remains fixed relative to the inertial frame. 

(b) Consider the motion of three coordinates frames, A , B 1 and 
C. Show that the following relationship holds between their 
hybrid velocities: 

Vac = Ad {-R abPbc ) V£ b + Ad# oi) V b h c . 

where Ad p denotes the adjoint map corresponding to a pure 
translation by p and Ad# denotes the adjoint map correspond¬ 
ing to pure rotation. 

(c) Show that the hybrid velocity of a rigid body is independent 
of the position of the spatial frame, but not its orientation. 

(d) Show that the hybrid velocity of a rigid body is independent 
of the orientation of the body frame, but not its position. 

(e) Interpret a wrench in hybrid coordinates and calculate the 
change of basis formulas for a change in spatial and/or body 
frames. 
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Chapter 3 

Manipulator Kinematics 


The kinematics of a robot manipulator describes the relationship between 
the motion of the joints of the manipulator and the resulting motion of 
the rigid bodies which form the robot. This chapter gives a description 
of the kinematics for a general n degree of freedom, open-chain robot 
manipulator using the tools presented in Chapter 2. We also present a 
brief treatment of redundant and parallel manipulators using this same 
framework. 


1 Introduction 

Most modern manipulators consist of a set of rigid links connected to¬ 
gether by a set of joints. Motors are attached to the joints so that the 
overall motion of the mechanism can be controlled to perform a given 
task. A tool, typically a gripper of some sort, is attached to the end of 
the robot to interact with the environment. 

Although any type of joint mechanism can be used to connect the links 
of a robot, traditionally the joints are chosen from a set of six mechanisms 
called lower pairs. These special types of joint mechanisms correspond to 
subgroups of the special Euclidean group SE{ 3). They represent revolute, 
prismatic, helical, cylindrical, spherical, and planar joints. 

The revolute , prismatic, and helical joints each correspond to screw 
motions, with the helical joint corresponding to a general screw with fi¬ 
nite, nonzero pitch. A cylindrical joint has two independent degrees of 
freedom and is typically constructed by combining a revolute and a pris¬ 
matic joint such that their axes are coincident. Revolute and prismatic 
joints are by far the most common type of joint encountered in robotics. 

A spherical joint is a mechanism which is capable of arbitrary rota¬ 
tions. Passive spherical joints often consist of a ball inserted into a socket, 
and are therefore referred to as ball and socket joints. Unfortunately, this 
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type of mechanism does not work well if the joint is to exert forces and 
torques, and hence actuated spherical joints are most often constructed 
by combining three revolute joints (with motors) such that their axes all 
intersect at a point. The orientation of the joint is then given by 

J Cj> _ gtDlfll g 2 2 02 ^383 

where uii,u) 2 ,u >3 £ R 3 represent the directions of the three axes. This is 
very similar to an Euler angle parameterization of orientation and has 
the same limitations in terms of singularities of the mechanism. Spher¬ 
ical mechanisms are often used as wrists in robot manipulators to allow 
arbitrary orientation of the gripper or tool at the end of the robot. 

Planar joints allow for arbitrary translation and rotation in the plane. 
Along with helical joints, they are the least commonly used of the lower- 
pair mechanisms. A planar joint can be built from a revolute joint at¬ 
tached to two independent prismatic joints. The motion of a planar joint 
is restricted to SE( 2), regarded as a 3-dimensional subgroup of SE(3). 

Modern robot manipulators, and kinematic mechanisms in general, 
are typically constructed by connecting different lower-pair joints together 
using rigid links. Since each of the joints restricts the motion of adjacent 
links to a subgroup of SE( 3), the tools developed in the last chapter 
provide a natural starting point for the analysis of such mechanisms. In 
this chapter and the next, we concentrate on the kinematics, dynamics, 
and control of open-chain robot manipulators, in which a number of links 
are attached serially by a set of actuated joints. By controlling the forces 
and torques on each of the links, we seek to move the robot in a specified 
way. 

The heart of the formulation which we present here is the product of 
exponentials formula , which represents the kinematics of an open-chain 
mechanism as the product of exponentials of twists. This setting works 
whenever the joints of the robot consist of either revolute, prismatic, or 
helical joints, which is the case for practically all commercially available 
robot manipulators. It provides a global, geometric representation of the 
kinematics of a manipulator which greatly simplifies the analysis of the 
mechanism and provides a very structured parameterization for open- 
chain robots. 

This chapter is organized as follows: Section 2 contains a derivation 
of the product of exponentials formula for the forward kinematics of an 
arbitrary open-chain manipulator. We concentrate on the most general 
case, where the end-effector configuration lies in SE( 3). Section 3 dis¬ 
cusses the inverse problem of finding a set of joint angles which causes 
the end-effector to have a desired configuration. We make extensive use 
of a set of subproblems originally proposed by Paden and Kahan which 
are very closely related to the exponential representation of rigid body 
motion. Section 4 derives the velocity and force relationships between 
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(prismatic) 


(revolute) 


Figure 3.1: Numbering conventions for an AdeptOne robot. 


the end-effector and the joints, and introduces the manipulator Jacobian 
for a robot. Finally, in Section 5 we extend some of the main results of 
this chapter to redundant manipulators and parallel mechanisms. 


2 Forward Kinematics 

2.1 Problem statement 

The forward kinematics of a robot determines the configuration of the 
end-effector (the gripper or tool mounted on the end of the robot) given 
the relative configurations of each pair of adjacent links of the robot. In 
this section, we restrict ourselves to open-chain manipulators in which 
the links form a single serial chain and each pair of links is connected 
either by a revolute joint or a prismatic (sliding) joint. To fix notation, 
we number the joints from 1 to n, starting at the base, and number the 
links such that joint i connects links i — 1 and i. Link 0 is taken to be 
the base of the manipulator and link n is attached rigidly to the end- 
effector. Figure 3.1 illustrates our choice of notational conventions for an 
AdeptOne robot (a type of SCAR A manipulator). 

The joint space Q of a manipulator consists of all possible values of 
the joint variables of the robot. This is also the configuration space of 


83 






















the robot, since specifying the joint angles specifies location of all of the 
links of the robot. For revolute joints, the joint variable is given by an 
angle 0 j £ [0, 2w) with the angle 2tt equated to the angle 0. This set of 
joint angles is naturally associated with a unit circle in the plane, denoted 
S 1 , and hence we write 9i £ S 1 for revolute joints. We measure all joint 
angles using a right-handed coordinate system, so that an angle about 
a directed axis is positive if it represents a clockwise rotation as viewed 
along the direction of the axis. Prismatic joints are described by a linear 
displacement 9^ £ R along a directed axis, where positive displacement is 
taken along the direction of the axis. 

The joint space Q is the Cartesian product between each of these 
individual joint spaces. The number of degrees of freedom of an open- 
chain manipulator is equal to the the number of joints in the manipulator. 
For the four degree of freedom SCARA robot of Figure 3.1, for instance, 
we have 9 = ( 01 , 02 , 03 , 04 ) £ S 1 x S 1 x I x S 1 = Q. For manipulators 
with multiple revolute joints, we use T p to represent the p-torus, defined 
to be the Cartesian product of p copies of S 1 : P = S 1 x ■ • ■ x S 1 . The 
joint space of a manipulator with p revolute joints and r prismatic joints 
is Q = T p x and has p + r degrees of freedom. In practice, Q may be 
defined to be a subset of this unrestricted joint space in order to account 
for joint constraints such as finite displacements and rotations. 

We attach two coordinate frames to the manipulator, as illustrated in 
Figure 3.1. The base frame, S, is attached to a point on the manipulator 
which is stationary with respect to link 0. Usually, S is attached directly 
to link 0, although this need not be the case, as we shall see later. (The 
reason for the use of the letter S instead of B is to avoid confusing the 
base frame with the body frame, which is ordinarily attached to a moving 
object.) The tool frame , T, is attached to the end-effector of the robot, 
so that the tool frame moves when the joints of the robot move. 

The forward kinematics problem can now be formalized. For simplic¬ 
ity, we refer to all joint variables as angles, although both angles and 
displacements are allowed, depending on the joint type. Given a set of 
joint angles 0 £ Q, we wish to determine the configuration of the tool 
frame T relative to the base frame S. The forward kinematics is repre¬ 
sented by a mapping g st : Q —> SE{ 3) which describes this relationship. 
The goal of this section is to show how to explicitly construct g s t for 
a given open-chain robot manipulator and explore the structure of this 
mapping. 

Classically, the forward kinematics map for an open-chain manipula¬ 
tor is constructed by composing the rigid motions due to the individual 
joints. Consider, for example, the two degree of freedom manipulator 
shown in Figure 3.2. To compute the configuration of the tool frame T 
relative to the base frame S, we concatenate the rigid motions between 
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adjacent frames: 


9st{0i,0 2 ) = g s i 1 ( d i)9hi 2 ( d 2)gi 2 t- 

The mapping g st : T 2 —► SE( 3) represents the forward kinematics of the 
manipulator: it gives the end-effector configuration as a function of the 
joint angles. 

This procedure is easily extended to any open-chain mechanism. If we 
define gi^i^Qi) as the transformation between the adjacent link frames, 
then the overall kinematics are given by 

9at{0) = gsiAV^giihi 0 ?) ■ ■ ■ 9i n -iin( 9 n)gi n t- (3.1) 

Equation (3.1) is a general formula for the forward kinematics map of an 
open-chain manipulator in terms of the relative transformations between 
adjacent link frames. 

2.2 The product of exponentials formula 

A more geometric description of the kinematics can be obtained by using 
the fact that motion of the individual joints is generated by a twist associ¬ 
ated with the joint axis. Recall that if £ is a twist, then the rigid motion 
associated with rotating and translating along the axis of the twist is 
given by 

9ab(0) = e&g ab ( 0 ). 

If £ corresponds to a prismatic (infinite pitch) joint, then 8 £ R. is the 
amount of translation; otherwise, 8 £ S 1 measures the angle of rotation 
about the axis. 
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Consider again the two degree of freedom manipulator shown in Fig¬ 
ure 3.2. Suppose that we fix the first joint and consider the configuration 
of the tool frame as a function of 9 2 only. This is a simple revolute (zero- 
pitch) screw motion about the axis of the second joint and hence we can 
write _ 

9st(0 2 ) = e^ e2 g st ( 0 ), 

where £2 is the twist corresponding to rotation about the second joint. 
Next, fix 0 2 and move only 9\. By composition, the end-effector configu¬ 
ration becomes 

g s t(0 1 ,e 2 ) = e^g st (9 2 ) = / 1@1 / 2 % st ( 0), (3.2) 

where £1 is the twist associated with the first joint. Equation (3.2) is an 
alternative formula for the manipulator forward kinematics. Note that £1 
and £2 are constant twists obtained by evaluating the screw motion for 
each joint at the 9i = 0 2 = 0 configuration of the manipulator. 

The simple form of equation (3.2) appears to rely on moving 9 2 first, 
followed by 9 \. This allowed us to represent the joint motions as twists 
about constant axes. To show that this representation does not depend 
on the order in which we move the joints, we can derive the forward 
kinematics by moving 9\ first, and then 0 2 . In this case, 

9*t(0i) =e^g st (0) 

is the motion due to moving 9\ with 9 2 fixed. This motion moves the 
axis of 9 2 , and rotation of the second link occurs around a new axis, 

£2 = Ad^ £2. 

Using the properties of the matrix exponential (see Exercise 8 in Chap¬ 
ter 2), the rigid body transformation 

e&® 2 

describes motion about the 

g s t{9i, 9 2 ) = 


?202j g —£l01 

new axis. Thus, 

e& 2 e^g st ( 0 ) 

e €iei e-^ l6l e^ iei g st ( 0) 

/ 1<?1 / 2 % st ( 0), 



as before. 

We can generalize this procedure to find the forward kinematics map 
for an arbitrary open-chain manipulator with n degrees of freedom. Let 
5 be a frame attached to the base of the manipulator and T be a frame 
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attached to the last link of the manipulator. Define the reference con¬ 
figuration of the manipulator to be the configuration of the manipulator 
corresponding to 9 = 0 and let g s t(0) represent the rigid body trans¬ 
formation between T and S when the manipulator is in its reference 
configuration. For each joint, construct a twist which corresponds to 
the screw motion for the ith joint with all other joint angles held fixed at 
6j = 0. For a revolute joint, the twist has the form 


& 


-u>i x qi 

OJi 


where aij G R 3 is a unit vector in the direction of the twist axis and 
qi G R 3 is any point on the axis. 1 * For a prismatic joint, 


6 = 


Vi 

0 


where Vi G R 3 is a unit vector pointing in the direction of translation. 
All vectors and points are specified relative to the base coordinate frame 
S. 

Combining the individual joint motions, the forward kinematics map, 
9st : Q —> SE( 3), is given by 


g s t{0)=e^e^ ■■■e^ K g st { 0) 


(3.3) 


The &’s must be numbered sequentially starting from the base, but g s t{0) 
gives the configuration of the tool frame independently of the order 
in which the rotations and translations are actually performed. Equa¬ 
tion (3.3) is called the product of exponentials formula for the manipulator 
forward kinematics. 


Example 3.1. SCARA forward kinematics 

Consider the SCARA manipulator shown in Figure 3.3. It consists of 
four joints—three revolute and one prismatic (note that we have chosen 
to order the joints differently than for the AdeptOne robot in Figure 3.1). 
We let 9 = 0 correspond to the fully extended configuration and attach 
base and tool frames as shown in the figure. 

The transformation between tool and base frames at 8 = 0 is given 


by 


9st{ 0) 


I 

0 


Ot' 2 ) 

1 


1 We choose the convention —cu X q instead of q X uj since the former can be correctly 

interpreted in both the spatial and planar cases (see Exercise 11 in Chapter 2). 
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Figure 3.3: SCARA manipulator in its reference configuration. 


To construct the twists for the revolute joints, note that 


Wl = U>2 = W3 = 


and we can choose axis points 



'o' 


'o' 


0 

qi = 

0 

92 = 

h 

93 = 

h + h 


0 


0 


0 


This yields twists 

a = 

The prismatic joint points in the z direction and has an associated twist 

U = 


"0" 


h 


Z 1 +Z 2 

0 


0 


0 

0 

0 

to 

II 

0 

0 

a = 

0 

0 

0 


0 


0 

_ 1. 


L i J 


L 1 J 




"0" 

V 4 


0 

1 

0 


0 

0 



LoJ 


The forward kinematics map of the manipulator has the form 
g st (d) =e^e^e& e *e^ ei g st { 0) = ^ P ^ 
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The individual exponentials are given by 


e tith 


e^ 2 


e^ 3 


e US4 


cos 9\ — sin 9i 0 

sin 9\ cos 9\ 0 

0 0 1 

0 0 0 

cos 9 2 — sin 0 2 0 

sin 0 2 cos 02 0 

0 0 1 

0 0 0 

cos 03 — sin 0 3 0 

sin 0 3 cos 0 3 0 

0 0 1 

0 0 0 

1 0 0 0 ' 

0 10 0 

0 0 1 04 ' 

0 0 0 1 


0 

0 

0 

1 

1 1 sin 02 

Zi (1 — COS 0 2 ) 

0 

1 

(h + l 2 ) sin 0 3 

(^1 + ^)(1 — COS 03) 
0 
1 


Expanding the terms in the product of exponentials formula yields 


9st(0) 


R(9) 

p(9) 


R(9) p(9) 

0 1 

cos(0i + 02 + 03) - sin(0i + 0 2 + 03) 0 
sin (0i + 02 + 03) cos(0i + 02 + 03) 0 

0 0 1 

—li sin 0i — l 2 sin(0i + 0 2 ) 
l\ COS 01 + l 2 cos(0i + 02) 

Iq + 04 


(3.4) 


Example 3.2. Elbow manipulator forward kinematics 

Consider the elbow manipulator shown in Figure 3.4. The mechanism 
consists of two intersecting axes at the shoulder, an elbow, and a spherical 
wrist (modeled as three intersecting axes). The reference configuration 
(0 = 0) is fully extended, as shown. 

The forward kinematics is computed by calculating the individual 
twist motions for each joint. The transformation between the tool and 
base frames at 0 = 0 is given by 


9st{ 0 ) 


I 

0 



The first two joints have twists 
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Figure 3.4: Elbow manipulator. 


Note that we have used q\ = (0, 0, Iq) for the first twist; we could just as 
well have used the origin or any other point on the axis of the twist. The 
other twists are calculated in a similar manner: 


£3 = 


- 0 - 


ll+l2 


- 0 - 


— lo 

— lo 


0 


— ^o 


0 

h 

-1 

</» 

II 

0 

0 

<?5 = 

Z 1 +Z 2 

-1 

& = 

0 

0 

0 


0 


0 


1 

L 0 -1 


L 1 J 


L 0 J 


0 J 


The full forward kinematics are 


g st (d) = e^---e^g st ( 0 ) = 


R(9) p(0) 
0 1 


where 



ru 

ri2 

ri3 

m = 

r2i 

^22 

T23 


J31 

^32 

r33 


— sin 9\{l 

1 cos 

p{0) = 

COS 

0i(h 

cos 6 


k 

— h 

sin 9 

the notation Cj 

= COS0i, 


^2 + ^2 cos ($2 


+ 03 ))' 

F0 3 )) 

-e 3 ) 


= sin 9i, Cij = cos (9i + 0j), Sij 
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sin(6»i + 9j), 

ril = Cq(ciC4 — S1C23S4) + S6(siS23C5 + S1C23C4S5 + C1S4S5) 

T 12 = —C5(siC23C4 + C1S4) + S1S23S5 

T 13 = Cq( — C5S1S23 — (C23C4S1 + CiS4)S5) + (C1C4 — C23SiS4)S6 

J"21 = Cq(C4Si + C1C23S4) — (C1C5S23 + (C1C23C4 — SiS4)S5)S6 
T 22 = C5 (C1C23C4 — S1S4) — C1S23S5 

^23 = C6(CiC5S23 + (C 1 C 23 C 4 — SiS 4 )S 5 ) + (C 4 S 1 + CiC23S4)S6 

^31 = — (C6S23S4) — (C23C5 — C4S23S5)fi6 

^32 = — (C4C5S23) — C23S5 

^33 = C6(C23C5 — C 4 S 23 S 5 ) — S23S4S6- 


2.3 Parameterization of manipulators via twists 

Using the product of exponentials formula, the kinematics of a manip¬ 
ulator is completely characterized by the twist coordinates for each of 
the joints. We now consider some issues related to parameterizing robot 
motion using twists. 

Choice of base frame and reference configuration 

In the examples above, we chose the base frame for the manipulator to 
be at the base of the robot. Other choices of the base frame are possible, 
and can sometimes lead to simplified calculations. One natural choice is 
to place the base frame coincident with the tool frame in the reference 
configuration. That is, we choose a base frame which is fixed relative to 
the base of the robot and which lines up with the tool frame when 0 = 0. 
This simplifies calculations since g s t{ 0) = I with this choice of base frame 
and hence _ _ 

9st(0) =e^ ei 

A further degree of freedom in specifying the manipulator kinematics 
is the choice of the reference configuration for the manipulator. Recall 
that the reference configuration was the configuration corresponding to 
setting all of the joint variables to 0. By adding or subtracting a fixed 
offset from each joint variable, we can define any configuration of the 
manipulator as the reference configuration. The twist coordinates for 
the individual joints of a manipulator depend on the choice of reference 
configuration (as well as base frame) and so the reference configuration is 
usually chosen such that the kinematic analysis is as simple as possible. 
For example, a common choice is to define the reference configuration 
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Figure 3.5: SCARA manipulator in its reference configuration, with base 
frame coincident with tool frame. 


such that points on the twist axes for the joints have a simple form, as 
in all of the examples above. 


Example 3.3. SCARA forward kinematics with alternate base 
frame 

Consider the SCARA manipulator with base frame coincident with the 
tool frame at 9 = 0, as shown in Figure 3.5. The twists are now calculated 
with respect to the new base frame: 



and similar calculations yield: 


"0" 


"o' 

0 


0 

0 

0 

</» 

II 

1 

0 

0 


0 

_ 1. 


L o J 
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Expanding the product of exponentials formula gives 


9st{0) 


R(9) 

P(0) 


R(9) p(9) 

0 1 

COs(0i + 02 + 03) - sin(0i + 02 + 03) 0 

sin(0! + 0 2 + 9 3 ) cos(0i + 0 2 + 9 3 ) 0 

0 0 1 

—li sin 0i — l 2 sin(0i + 0 2 ) 

—— 1 2 ~b COS 0i -f- l 2 COs(0i T 02) 

04 


(3.5) 


Note that g st { 0) = /, which is consistent with the fact that the base 
and tool frames are coincident at 0 = 0. Compare this formula with the 
kinematics map derived in Example 3.1. 


Relationship with Denavit-Hartenberg parameters 

Given a base frame S and a tool frame T , the coordinates of the twists 
corresponding to each joint of a robot manipulator provide a complete 
parameterization of the kinematics of the manipulator. An alternative 
parameterization, which is the de facto standard in robotics, is the use of 
Denavit-Hartenberg parameters [25]. In this section, we discuss the rela¬ 
tionships between these two parameterizations and their relative merits. 

Denavit-Hartenberg parameters are obtained by applying a set of rules 
which specify the position and orientation of frames Li attached to each 
link of the robot and then constructing the homogeneous transformations 
between frames, denoted gi i _ 1 i i . By convention, we identify the base 
frame S with Lq. The kinematics of the mechanism can be written as 

g s t{0) = gi 0 h(9i)ghh(Q2) ■ ■ ■ gi n -i,ir,(Qn)gi nt , (3.6) 

just as in equation (3.1). Each of the transformations gi i _ 1} i i has the form 

sin fa sin a* ai cos (j>i 
- cos fa sin ai at sin fa 
cos ai fa 

0 1 

where the four scalars a*, a*, di , and 4>i are the parameters for the ith 
link. For revolute joints, fa corresponds to the joint variable 0j, while 
for prismatic joints, di corresponds to the joint variable 0^. Denavit- 
Hartenberg parameters are available for standard industrial robots and 
are used by most commercial robot simulation and programming systems. 

It may seem somewhat surprising that only four parameters are needed 
to specify the relative link displacements, since the twists for each joint 
have six independent parameters. This is achieved by cleverly choosing 


9h-i,h = 


cos fa — sin (f>i cos ai 
sin fa cos fa cos ai 
0 sin at 

0 0 
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the link frames so that certain cancellations occur. In fact, it is possible 
to give physical interpretations to the various parameters based on the 
relationships between adjacent link frames. An excellent discussion can 
be found in Spong and Vidyasagar [110]. 

There is not a simple one-to-one mapping between the twist coordi¬ 
nates for the joints of a robot manipulator and the Denavit-Hartenberg 
parameters. This is because the twist coordinates for each joint are speci¬ 
fied with respect to a single base frame and hence do not directly represent 
the relative motions of each link with respect to the previous link. To 
see this, let £i~i t i be the twist for the ith link relative to the previous link 
frame. Then, gi i _ 1 j i is given by 

(3-7) 

and the forward kinematics map becomes 

9st{0) =e«”Sw 1 (0) e^g hl2 ( 0) ••• e^-^ e -g ln _ lln ( 0). (3.8) 

This is evidently not the same as the product of exponentials formula, 
though it bears some resemblance to it. 

The relationship between the twists and the pairs (0) and 

fi-i.i can be determined using the adjoint mapping. We first rewrite 
equation (3.8) as 


9st(0) = (^(O^-S^O)) ■ 

(to^^Ko)) • ■ ■ (5io^_ 1 (0)e^- 1 - e " 5 J„_ 1 (0)) Si o t(0). (3.9) 
We can simplify this equation by making use of the relationship 

ge&g- 1 = 


to obtain 


g st {9) = ?12)A02 


o ( Ad 9! 0 ! n _l(°) fn-l.n/ 


0Zot(°)- 


It follows immediately that 


fi — £j-l ,i- (3.10) 

This formula verifies that the twist G is the joint twist for the ith joint 
in its reference configuration and written relative to the base coordinate 
frame. 

Given the Denavit-Hartenberg parameters for a manipulator, the cor¬ 
responding twists can be determined by first parameterizing gt-i^i 
using exponential coordinates, as in equation (3.7), and then applying 
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equation (3.10). However, in almost all instances it is substantially easier 
to construct the joint twists £; directly, by writing down the direction of 
the joint axes and, in the case of revolute joints, choosing a convenient 
point on each axis. Indeed, one of the most attractive features of the prod¬ 
uct of exponentials formula is its usage of only two coordinate frames, the 
base frame 5, and the tool frame T. This property, combined with the 
geometric significance of the twists £$, make the product of exponentials 
representation a superior alternative to the use of Denavit-Hartenberg 
parameters. 

2.4 Manipulator workspace 

The workspace of a manipulator is defined as the set of all end-effector 
configurations which can be reached by some choice of joint angles. If Q 
is the configuration space of a manipulator and g st : Q —> 55(3) is the 
forward kinematics map, then the workspace W is defined as the set 

W = {g st (0) : 9 G Q} C SE( 3). (3.11) 

The workspace is used when planning a task for the manipulator to ex¬ 
ecute; all desired motions of the manipulator must remain within the 
workspace. We refer to this notion of workspace as the complete workspace 
of a manipulator. 

Characterizing the workspace as a subset of SE( 3) is often somewhat 
difficult to interpret. Instead, one can consider the set of positions (in 
R 3 ) which can be reached by some choice of joint angles. This set is called 
the reachable workspace and is defined as 

W R = {p(0) :0&Q} CM 3 , (3.12) 

where p(9) : Q —> R 3 is the position component of the forward kinematics 
map g st . The reachable workspace is the volume of R 3 which can be 
reached at some orientation. 

Since the reachable workspace does not consider ability to arbitrarily 
orient the end-effector, for some tasks it is not a useful measure of the 
range of a manipulator. The dextrous workspace of a manipulator is the 
volume of space which can be reached by the manipulator with arbitrary 
orientation: 

W D = {p G R 3 : \/R G 50(3), 3 0 with g st {9) = (p, R)} c R 3 . (3.13) 

The dextrous workspace is useful in the context of task planning since it 
allows the orientation of the end-effector to be ignored when positioning 
objects in the dextrous workspace. 

For a general robot manipulator, the dextrous workspace can be very 
difficult to calculate. A common feature of industrial manipulators is to 
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Figure 3.6: Workspace calculations for a planar three-link robot (a). 
The construction of the workspace is illustrated in (b). The reachable 
workspace is shown in (c) and the dextrous workspace is shown in (d). 


place a spherical wrist at the end of the manipulator, as in the elbow 
manipulator given in Example 3.2. Recall that a spherical wrist consists 
of three orthogonal revolute axes which intersect at a point. If the end- 
effector frame is placed at the origin of the wrist axes, then the spherical 
wrist can be used to achieve any orientation at a given end-effector po¬ 
sition. Hence, for a manipulator with a spherical wrist, the dextrous 
workspace is equal to the reachable workspace, Wo = Wr. Furthermore, 
the complete workspace for the end-effector satisfies W = Wr x 50(3). 
This analysis only holds when the end-effector frame is placed at the 
center of the spherical wrist; if an offset is present, the analysis becomes 
more complex. 

Example 3.4. Workspace for a planar three-link robot 

Consider the planar manipulator shown in Figure 3.6a. Let g = (x,y,cj)) 
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represent the position and orientation of the end-effector. The forward 
kinematics of the mechanism can be derived using the product of expo¬ 
nentials formula, but are more easily derived using plane geometry: 

x = h cos 0i + l 2 cos(0i + 0 2 ) + h cos(0i + 0 2 + 0 3 ) 
y — l\ sin 0i + l 2 sin(0i + 6 2 ) + l 3 sin(0i + 02 + O 3 ) (3.14) 

(/> = 01 + 02 + 03 ■ 

We take l\> l 2 > l 3 , and assume l\ > l 2 +1 3 . 

The reachable workspace is calculated by ignoring the orientation of 
the end-effector. To generate it, we first take 0i and d 2 as fixed. The 
set of reachable points becomes a circle of radius l 3 formed by sweeping 
0 3 through all angles (see Figure 3.6b). We now let 0 2 vary through all 
angles to get an annulus with inner radius l 2 — 1 3 and outer radius l 2 + 1 3 
centered at the end of the first link. Finally, we generate the reachable 
workspace by sweeping the annulus through all values of 0i, to give the 
reachable workspace. The final construction is shown in Figure 3.6c. Wr 
is an annulus with inner radius l\ — l 2 — l 3 and outer radius l\ + 1 2 + l 3 . 

The dextrous workspace for this manipulator is somewhat subtle. Al¬ 
though the manipulator has the planar equivalent of a spherical wrist, 
the end-effector frame is not aligned with the center of the wrist. This 
reduces the size of the dextrous workspace by 2 1 3 on the inner and outer 
edges, as shown in Figure 3.6d. 


3 Inverse Kinematics 

We now consider the inverse kinematics problem: given a desired config¬ 
uration for the tool frame, find joint angles which achieve that configu¬ 
ration. That is, given a forward kinematics map g st : Q —> SE( 3) and a 
desired configuration gd £ SE( 3), we would like to solve the equation 

g s t(0) = g d (3.15) 

for some 0 £ Q. This problem may have multiple solutions, a unique 
solution, or no solution. 

3.1 A planar example 

To illustrate some of the issues in inverse kinematics, we first consider 
the inverse kinematics of the planar two-link manipulator shown in Fig¬ 
ure 3.7a. The forward kinematics can be determined using plane geome¬ 
try: 

x = l\ cos 0i + l 2 cos(0i + d 2 ) 
y = li sin 0! + Z 2 sin(0i + 0 2 ). 
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(a) 


(b) 


Figure 3.7: Inverse kinematics of a planar two-link manipulator. 

The inverse problem is to solve for 9\ and 62 , given x and y. A standard 
trick is to solve the problem using polar coordinates, (r, </>), as shown in 


Figure 3.7b. From this viewpoint, 62 is determined by r = \Jx 2 + y 2 , 
and the law of cosines gives 



(3.16) 


If a 0, there are two distinct values of 82 which give the appropriate 
radius; the second is referred to as the “flip solution” and is shown as a 
dashed line in Figure 3.7b. The complete solution is given by solving for 
</> and using this to determine 0\. This problem must be solved for each 
possible value of 0 2l yielding 



where the sign used for /3 agrees with that used for a. 

This planar example illustrates several important features of inverse 
kinematics problems. In solving an inverse kinematics problem, one first 
divides the problem into specific subproblems, such as solving for 82 given 
r and then using 8 \ to rotate the end-effector to the proper position. 
Each subproblem may have zero, one, or many solutions depending on 
the desired end-effector location. If the configuration is outside of the 
workspace of the manipulator, then no solution can exist and one of 
the subproblems must fail to have a solution (consider what happens 
if r > li + I 2 in the example above). Multiple solutions occur when 
the desired configuration is within the workspace but there are multiple 
joint configurations which all map to the same end-effector location. If 
a subproblem generates multiple solutions, then we must complete the 
solution procedure for all joint angles generated by the subproblem. 
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Traditionally, inverse kinematics solutions are separated into classes: 
closed-form solutions and numerical solutions. Closed-form solutions, 
such as the one given above, allow for fast and efficient calculation of the 
joint angles which give a desired end-effector configuration. Numerical 
solutions rely on an interactive procedure to solve equation (3.15). Most 
industrial manipulators have closed-form solutions. These solutions are 
obtained in a manner similar to that described above: using geometric 
and algebraic identities, solve the set of nonlinear, coupled, algebraic 
equations which define the inverse kinematics problem. An introduction 
to classical techniques for solving inverse kinematics problems is given in 
Craig [21]. 

3.2 Paden-Kahan subproblems 

Using the product of exponentials formula for the forward kinematics 
map, it is possible to develop a geometric algorithm to solve the inverse 
kinematics problem. This method was originally presented by Paden [85] 
and built on the unpublished work of Kahan [46]. 

To solve the inverse kinematics problem, we first solve a number of 
subproblems which occur frequently in inverse solutions for common ma¬ 
nipulator designs. One then seeks to reduce the full inverse kinematics 
problem into appropriate subproblems whose solutions are known. One 
feature of the subproblems presented here is that they are both geomet¬ 
rically meaningful and numerically stable. Note that this set of subprob¬ 
lems is by no means exhaustive and there may exist manipulators which 
cannot be solved using these canonical problems. Additional subproblems 
are explored in the exercises. 

For each of the subproblems presented below, we give a statement of 
the geometric problem to be solved and a detailed solution. On a first 
reading of this section, it may be difficult to understand the relevance of 
the specific subproblems presented here. We recommend that the first¬ 
time reader skip the solutions until she sees how the subproblems are 
used in the examples presented later in this section. 

Subproblem 1. Rotation about a single axis 

Let £ be a zero-pitch twist with unit magnitude and p,q £l 3 two points. 
Find 0 such that 

e&p = q. (3.17) 

Solution. This problem corresponds to rotating a point p about a given 
axis £ until it coincides with a second point q , as shown in Figure 3.8a. 
Let r be a point on the axis of £. Define u = (p — r) to be the vector 
between r and p, and v = (q — r) the vector between r and q. It follows 
from equation (3.17) and the fact that exp(£(9)r = r (since r is on the 
axis of f) that 

e Qe u = v , (3.18) 
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(a) 



(b) 


Figure 3.8: Subproblem 1: (a) Rotate p about the axis of £ until it is 
coincident with q. (b) The projection of u and v onto the plane normal 
to the twist axis. 


where we have used the fact that u and v are vectors, and hence we have 
exp(£0)u = exp (us9)u. 

To determine when the problem has a solution, define u' and v' to be 
the projections of u and v onto the plane perpendicular to the axis of £. 
If w £ K 3 is a unit vector in the direction of the axis of £, then 

u' = u — usus T u and v' = v — usus T v. 


The problem has a solution only if the projections of u and v onto the 
w-axis and onto the plane perpendicular to us have equal lengths. More 
formally, if we project equation (3.18) onto the span of us and the null 
space of us T , we obtain the necessary conditions 

us T u = us T v and ||u , |[ = ||u , ||. (3.19) 


If equation (3.19) is satisfied, then we can find 9 by looking only at 
the projected vectors u' and v' as shown in Figure 3.8b. If u' ^ 0, then 
we can determine 8 using the relationships 


u' X v' = wsin0||u , ||||u / || 
u ■ v' = cos0||u , ||||u / || 


9 = atan2(w T (u / x v'),u' T v'). 


If v! = 0, then there are an infinite number of solutions since, in this case, 
p = q and both points lie on the axis of rotation. □ 


Subproblem 2. Rotation about two subsequent axes 

Let and £2 be two zero-pitch , unit magnitude twists with intersecting 
axes and p,q G R 3 two points. Find 8\ and @2 such that 


e «i 0 i e «2 e 2p = 


q- 


(3.20) 
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Figure 3.9: Subproblem 2: Rotate p about the axis of £2 followed by a 
rotation around the axis of such that the final location of p is coincident 
with q. 


Solution. This problem corresponds to rotating a point p first about the 
axis of £2 by 82 and then about the axis of £1 by Q\, so that the final 
location of p is coincident with the point q. This motion is depicted in 
Figure 3.9. If the axes of £1 and £2 coincide, this problem reduces to 
Subproblem 1 and any 8 \, 62 such that 8 \ + 62 = 0 is a solution, where 8 
is the solution to Subproblem 1. 

If the two axes are not parallel, i.e., W 1 XU 2 / 0, then let c be a point 
such that 

p « 2«2 p = c = e -ii8 lq ( 3 . 2 !) 


L q- 


In other words, c represents the point to which p is rotated about the 
axis of £2 by 8 2 . Let r be the point of intersection of the two axes, so 
that 

e ^ 202 (p — r) = c — r = (q — r). (3.22) 

As before, define vectors u = (p — r), v — (q — r), and z = (c — r). 
Substituting these expressions into equation (3.22) gives 


e £ 2 % = ^ = e _Sl %, 


which implies that 


u> 2 U = 1 ^ 2 z and uiiV=uj^z (3.23) 

and ||it || 2 = ||^|| 2 = ||u|| 2 . Furthermore, since uq, w 2 , and u 1 x w 2 are 
linearly independent, we can write 

z = awi + / 3 ui 2 + 7 ( 0*1 x 0 * 2 ) (3.24) 
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and 


|| z || 2 — o? + f3 2 + 2a/3w^ uj 2 + q 2 ||wi x u >2 1| 2 • (3.25) 

Substituting equation (3.24) into equation (3.23) gives a system of two 
equations in two unknowns: 


ui 2 u = au>2 uq + (3 
v = a + fluii u>2 


a = 

/? = 


(cjf W 2 )wJ u — UJ 1 V 
(wfuq) 2 - 1 
(u>f U)2)u>i v — UI2 U 

(u}foj 2 ) 2 - 1 


Solving equation (3.25) for q 2 and using ||;z || 2 = ||w || 2 yields 


2 _ ||u || 2 - a 2 — (3 2 — 2a/3u'[uJ2 
||wi x w 2 || 2 


This equation may have zero, one, or two real solutions. In the case that 
a solution exists, we can find z —and hence c—given a, /?, and 7 . 

To find 91 and 0 2 , we solve 


e^ 2 ° 2 p = c 


and 


e ^ lSl q = c 


using Subproblem 1. If there are multiple solutions for c, each of these 
solutions gives a value for 9\ and 0 2 . Two solutions exist in the case 
where the circles in Figure 3.9 intersect at two points, one solution when 
the circles are tangential, and none when the circles fail to intersect. □ 


Subproblem 3. Rotation to a given distance 

Let £ be a zero-pitch, unit magnitude twist; p,q £ M 3 two points; and S a 
real number > 0. Find 9 such that 

Ik - e^kll = 6. (3.26) 

Solution. This problem corresponds to rotating a point p about axis £ 
until the point is a distance <5 from q , as shown in Figure 3.10a. A 
solution exists if the circle defined by rotating p around £ intersects the 
sphere of radius 5 centered at q. 

To find the explicit solution, we again consider the projection of all 
points onto the plane perpendicular to u, the direction of the axis of t;. 
Let r be a point on the axis of £, and define u = (p — r) and v = (q — r) 
so that 

\\v - e Qe u\\ 2 = S 2 . (3.27) 

The projections of u and v are 

u = u — uuj t u and v' = v — louj t v. 
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u T (p-<l) 


€ 


(a) 



(b) 


Figure 3.10: Subproblem 3: (a) Rotate p about the axis of £ until it is 
a distance <5 from q. (b) The projection onto the plane perpendicular to 
the axis. The dashed line is the “flip” solution. 


We can also “project” <5 by subtracting the component of p — q in the u> 
direction, 

5' 2 = 6 2 -\u, T (p-q)\ 2 , 
so that equation (3.27) becomes 

\\v' - e Qe u'\\ 2 = S' 2 

(see Figure 3.10b). If we let 9q be the angle between the vectors v! and 
v', we have 

9 0 = atan2 (co T (u' x v'),u' T v'). (3.28) 

We can now use the law of cosines to solve for the angle (j> = 9 q — 9. The 
triangle formed by the center of the axis, exp(o)0)it', and v' satisfies 

Hit'll 2 + ||i/|| 2 - 2||it'||||i/|| cost/) = 5' 2 


and therefore 


9 = 9q ± cos 1 




(3.29) 


Equation (3.29) has either zero, one, or two solutions, depending on the 
number of points in which the circle of radius ||it'|| intersects the circle of 
radius S '. fli 
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3.3 Solving inverse kinematics using subproblems 

Given the solutions to the subproblems presented above, we must now 
find techniques for converting the complete inverse kinematics problem 
into the appropriate subproblems. 

The basic technique for simplification is to apply the kinematic equa¬ 
tions to special points, such as the intersection of two or more axes. This 
is a potentially powerful operation since exp(£(9)p = p if p is on the axis 
of a revolute twist £. Using this, we can eliminate the dependence of 
certain joint angles by appropriate selection of points. For example, if we 
wish to solve 

_ g 

with £ 1 , £ 2 , and £3 all zero-pitch twists, then applying both sides to a 
point p on the axis of £3 yields 

e^ lSl e ^ 2 ® 2 p = gp , 

which can be solved using Subproblem 2 (in the case that £1 and £2 
intersect). 

Another common trick for reducing a problem to a subproblem is to 
subtract a point from both sides of an equation and take the norm of 
the result. Since rigid motions preserve norm, some dependencies can be 
eliminated. For example, if we wish to solve 

gCi^ig? 2 ^ 2 gC3®3 _ g 

for £ 3 , and £ 1 , £ 2 intersect at a point q, then we can apply both sides of 
the equation to a point p that is not on the axis of £3 and subtract the 
point q. Taking the norm of the result yields 

<5 : = ||gp — < 7 H = || e^ l6l e^ 2d2 e^ 3e3 p — g|| 

= \\e^ ei e^ 262 (e^ e3 p-q)\\ 

= ||- g|| 

which is Subproblem 3. 

We now show how the subproblems of Section 3.2 can be used to 
solve the inverse kinematics of some common manipulators. Additional 
examples appear in the exercises. 

Example 3.5. Elbow manipulator inverse kinematics 

The elbow manipulator in Figure 3.11 consists of a three degree of freedom 
manipulator with a spherical wrist. This special structure simplifies the 
inverse kinematics and fits nicely with the subproblems presented earlier. 
The equation we wish to solve is 

g s t(0) = e^---e^g st (O)=g d , 
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Figure 3.11: Elbow manipulator. 


where g,i £ SE( 3) is the desired configuration of the tool frame. Post- 
multiplying this equation by (/^(O) isolates the exponential maps: 

e ?^... e &°°= gdg - 1 (0)= :gi . (3.30) 

We determine the requisite joint angles in four steps. 

Step 1 (solve for the elbow angle, d 3 ). Apply both sides of equation (3.30) 
to a point p w £ M 3 which is the common point of intersection for the wrist 
axes. Since exp(^0)p w = p w if p w is on the axis of £, this yields 

= 9iPw (3-31) 

Subtract from both sides of equation (3.31) a point pb which is at the 
intersection of the first two axes, as shown in Figure 3.11: 

e ?i0i e ^ e ?30 3pw - Pb = e Ci e i e f202 ^ e ?30 3Vw _ pb j = giPw _ pbm (3.32) 

Using the property that the distance between points is preserved by rigid 
motions, take the magnitude of both sides of equation (3.32): 

II e^ 303 p w -Pb\\ = \\giPw ~ Pb \\• (3.33) 

This equation is in the form required for Subproblem 3, with p = p w , 
q = pb-, and 6 = \\gip w — Pb ||- Applying Subproblem 3, we solve for d 3 . 

Step 2 (solve for the base joint angles). Since d 3 is known, equation (3.31) 
becomes _ 

e £i8 le £ 2 e 2 ( e f 3 e 3 p w j = gi p W ' ( 3 . 34 ) 

Applying Subproblem 2 with p = exp(( 3 0 3 )p w and q = g\p w gives the 
values for 8\ and 8 3 . 
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Step 3 (solve for two of three wrist angles). The remaining kinematics 
can be written as 


e ue ie £ 5 e 5e £ 6 e s = e -(? 3 e 3 e -£ 2 e 2 gdg -i ^ =: g2 


(3.35) 


Apply both sides of equation (3.35) to a point p which is on the axis of 
^6 but not on the £ 4 , £5 axes. This gives 


e USi 


e is05p _ g 2 p 


(3.36) 


Apply Subproblem 2 to find #4 and 9 5 . 

Step 4 (solve for the remaining wrist angle). The only remaining un¬ 
known is 9q. Rearranging the kinematics equation and applying both 
sides to any point p which is not on the axis of £ 6 , 

= e -fs<h e -ue*... e -^ lBl g d g^{Q)p =: q. ( 3 . 37 ) 

Apply Subproblem 1 to find 9 6 . 

At the end of this procedure, 0\ through 9q are determined. There 
are a maximum of eight possible solutions, due to multiple solutions for 
equations (3.33), (3.34), and (3.36). Note that the overall procedure is to 
first solve for the three angles which determine the position of the center 
of the wrist and then solve for the wrist angles. 

Example 3.6. Inverse kinematics of a SCARA manipulator 

Consider the four degree of freedom SCARA manipulator shown in Fig¬ 
ure 3.12. From the forward kinematics derived in Example 3.1 on page 87, 
the tool configuration has the 


9st(0) = e^---e^g st ( 0 ) = 


and hence we can solve the inverse kinematics given x, y, z, and <j> as 
in equation (3.38). We begin by solving for # 4 . Applying both sides of 
equation (3.38) to the origin of the tool frame gives 


cos</> 
sin (f> 
0 
0 


— sin (j> 0 x 

cos <f> 0 y 

0 1 z 

0 0 1 


=: 9d 





—li sin#! — l 2 sin(#! + # 2 ) 


X 

p(6) = 

l\ COS #1 + l 2 COs(#i + # 2 ) 

= 

y 


1 

+ 

-S’ 


z 


where p{ 8 ) is the position component of the forward kinematics, given by 
equation (3.4). From the form of p(0), we see that 84 = z — Iq. Notice that 
finding 64 did not make use of any of the previously defined subproblems. 
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Figure 3.12: SCARA manipulator in its reference configuration. 


Once 04 is known, we can rearrange equation (3.38) to read 

e hOi e hez e he 3 = gd g-i( 0) e -t* e * =-. gi . ( 3 . 39 ) 

Let p be a point on the axis of £3 and q be a point on the axis of £ 4 . Ap¬ 
plying equation (3.39) to p, subtracting q from both sides, and applying 
norms, 

\\ e ^e^ 02 p- q\\ = || e « 101 (e« 2 % - g)|| ^ 

= II- q\\ = \\gip - <?|| =: 6. 

Application of Subproblem 3 gives the value of 62 . 0i can now be found 
by applying equation (3.39) to a point p' on the axis of £3 and solving 
Subproblem 1: 

e iiSi e he2 e ?36 3p ' = e ffi0i LZ20 2p '^ = gip i 

Finally, we rearrange equation (3.39), shifting the (known) 0i and 02 
terms to the right-hand side: 

e e 3 03 = e -^ 2 e 2 e-^ l 8 l g d g- t 1 {e)e-^ ei . (3.41) 

Applying equation (3.41) to any point p which is not on the axis of £3 and 
solving Subproblem 1 a final time gives 03 and completes the solution. 

There are a maximum of two possible solutions for the SCARA ma¬ 
nipulator, corresponding to the solutions of equation (3.40). 
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3.4 General solutions to inverse kinematics problems 

In the preceding sections, we have developed an elegant set of geometric 
and conceptual subproblcms in terms of which the inverse kinematics 
solution of a large number of manipulators can be decomposed. The set 
of subproblems and their extensions given in the exercises turn out to be 
useful for decomposing the solution of the inverse kinematics equations, 
primarily when the robot has at least some intersecting axes. 

The question of how to solve the inverse kinematics problem in gen¬ 
eral, (that is, in the absence of any intersections of the axes of the manipu¬ 
lator) for both planar and spatial mechanisms is an extremely active area 
of current research. In particular, there are many interesting questions 
about the number of inverse kinematics solutions and how the computa¬ 
tions can be mechanized in real-time. In this subsection, we give a brief 
summary of some of the newest and most general approaches in this re¬ 
gard, drawn from [57], [64] and [95]. Our development closely parallels 
that of Manocha and Canny [64]. The approaches are primarily based on 
classic elimination theory from algebraic geometry; this is a systematic 
procedure for simultaneously eliminating n — 1 of the variables in a sys¬ 
tem of n polynomials in n variables to obtain a single polynomial in one 
variable. This procedure is a general procedure but also a “brute-force” 
procedure, in that it only takes very simple properties of the manipulator 
kinematics into account (unlike the solutions based on the subproblems 
listed above). We illustrate this procedure, sometimes referred to as di- 
alytical elimination , in the following example of three nonhomogeneous 
polynomials in three variables. 


Example 3.7. Dialytical elimination for three polynomials in 
three variables 

We will assume that the three polynomials fi, f2, /3 are nonhomogeneous 
in xi, X 2 , x 3 and are of the form 


x 


2 ' 

1 


fi 


ai bi ci d\ ei 31 


X\X 2 


'O' 

h 

= 

a 2 b 2 c 2 d 2 e 2 g 2 


X\X 3 

-r 2 

= 

0 

h 


_a 3 b 3 c 3 d 3 e 3 g 3 _ 


x 2 


0 





x 3 




1 

Here, ai,bi,Ci,di,ei,gi, for i = 1,...,3 are all real numbers. To solve 
this system of three polynomials in xi,X 2 ,X 3 for their common zeros, we 
eliminate x 2 ,x 3 to get a single polynomial in xi in two steps: 


Step 1. We express the above equation as a system of equations in X 2 , £3 
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with coefficients being polynomials in X\ as follows: 


7 i" 


d\ e\ + ci£i b\X\ a\x\ + gi 


£2 


'o' 

h 

= 

d.2 e 2 + c 2 £i & 2 £i a 2 x\ + g 2 


^3 

= 

0 

h_ 


_d 3 e 3 + c 3 £i 6 3 £i a 3 x1 + g 3 _ 


X2 

1 


0 


Step 2 . We try to generate more polynomials which are independent 2 
of fi, f 2l f 3 until we have as many unknown monomials 3 in £2,3:3 as 
there are equations. For example, multiplying fi,f 2 ,f 3 by £2 yields the 
following set of polynomials which are independent of fi, f 2 , f 3 '- 


*2/1 

£2/2 

£2/3 


"di 

ei + ci£i 

bixi 

aixj + gi 


' x\ 


'O' 

d 2 

e 2 + c 2 £i 

b 2 x 1 

a 2 xl + g 2 


X 2 X 3 

£2 
. £2 

= 

0 

d 3 

e 3 + c 3 x 1 

b 3 x 1 

« 3 £? + 53 . 



0 


Combining these equations with those for /i,/2,/3 yields the following 
set of six equations in the six monomials £|, £^, £2£3, £2, £3,1 : 


A{x{) := 


~d 1 ei+c-ixi b 1X1 
d 2 e2+C2X! b 2 x 1 
d 3 e 3 +c 3 X! & 3*1 
0 0 di 

0 0 d 2 

.0 0 d 3 


ai^i+ 9 l 

a 2 xl+g 2 

“ 3^+93 

b 1 x 1 

b 2 xi 

b 3 xi 


0 0 

0 0 

0 0 

ei+cixi aixf+gi 
e 2 +c 2 xi a 2 x\+g 2 
e 3 +c 3 x 1 a 3 xl+g 3 . 


*i 


'o' 

£2£3 


0 

£’2 


0 

£2 


0 

£3 


0 

1 


0 


For this equation to have a solution, it is clear that the determinant 
of the matrix A(x{) multiplying the entries x\, X2X3, £|, £2, £3 ,1 should 
have zero determinant. Thus, the system of equations /1 = f 2 = f 3 = 0 
is equivalent to det-A(xi) = 0 . In general, the degree of this polynomial 
in £1 is high (generically, i.e., for almost all values of Oj,..., <?i, its degree 
is six) and, furthermore, solutions need not be real. However, the degree 
of the polynomial is an upper bound on the number of real solutions 
and once a real number £1 has been found, one can read off the solution 
for £2, £3 by scaling the element in the null space of A(x 1) to have 1 
as its last entry. If the null space of the matrix A(x 1) has dimension 
greater than one, corresponding to a multiplicity of roots for the poly¬ 
nomial detH(£i) = 0, then there may be a multiplicity of solutions for 
£2, £3 corresponding to the value(s) of £1 for which the null space of the 
matrix A(x 3 ) has dimension greater than 1 . While the exact multiplic¬ 
ity is a subtle question in [ 64 ], it is stated that an upper bound for the 
multiplicity of the solutions £2, £3 associated with that value of £1 is the 
dimension of the null space of A(x 1). 

independence is meant in a technical sense, over the ring of polynomials in xi- 

3 A monomial is a single polynomial; for example, X2X3 or x 3 . 
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A general procedure for six degree of freedom manipulators 


To apply this procedure to the inverse kinematics of a six-link manip¬ 
ulator, some preliminary work is necessary. The kinematics are given 

by 


g st {0) = e^---e^ eB g st ( 0 ) 


and the inverse kinematics problem is to solve for 0i,... given gd £ 
SE( 3). We rewrite the kinematics in terms of the Denavit-Hartenberg 
parameterization, as in equation (3.6): 


9st(9) — gi 0 h {0i)9hl 2 (^ 2 ) ’ ’ ’ 9hi e (0n)gi e t — 9d- 


By proper choice of link frames, each gi i _ 1 i i has the form 


9h-iM 


cos <pi 
sin fa 


0 

0 


— sin (j)i cos cq sin </>, sin cc* a; cos <pi 
cos (pi cos at — cos tpi sin an on sin pi 
sin aj cos a.i di 

0 0 1 


(3.42) 

Given a desired gd for the end-effector position and orientation, we rewrite 
this equation as 


9hh (03)5/3*4 (04)5*4*5 (05) 


5q; 2 (02)5; o q (0i) 9d 9i e t9i 5 i 6 (®6)- 


(3.43) 


What we have done is to take the inverses of gi 5 i 6 {0&), gi 0 i i(0i), and 
5 * 1*2 (02) and moved them to the right-hand side. The reason for this 
clever rearrangement and the choice of the Denavit-Hartenberg parame¬ 
terization in the kinematics will become clear in Step 1 of the procedure 
which follows. By way of notation, we write gugpjt as 


lx 

m x 

n x 

Qx 

ly 

m y 

n y 

5 y 

h 

m z 

n z 

9z 

0 

0 

0 

1 


(3.44) 


Hence, l,m,n £ R 3 are the columns of the rotational component of gd 9 i 6 \ 
and and q £ R 3 is the translational component. 

To bring to bear the machinery of algebraic geometry in an equation 
involving sines and cosines of angles, we define Cj := cos di and Sj := sin5i 
for i = 1,... ,6 and think of equation (3.43) as being polynomial in s,; 
and Cj. Indeed, by direct inspection we may see that the entries of each 
exponential are unary (i.e., of degree 1 or less) in Cj and s^. Part of the 
reason for rewriting the kinematics as in equation (3.43) is to be able to 
reduce the order of the polynomial in ,s,; and q. Both the left-hand side 
and right-hand side of (3.43) are now cubic in .s, and Cj. The details of 
the procedure to eliminate all the variables except for 63 is sketched as a 
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number of steps. The details of the proofs and justification of the steps 
is quite involved and, for them, the reader is referred to the papers cited 
above. 


Step 1. Verify that the third and fourth columns of equation (3.43) 
are independent of 9q. This is the step where the Denavit-Hartenberg 
parameterization comes in handy. Indeed, from taking the inverse of the 
formula for gi 6 t(9e) from equation (3.42), it follows that 




c 0 6 

s 8 e 

0 

CLq 

CotQ &0Q 

CctQ C0 6 

s ot& 

des a6 

Sa.6 ^Oq 

^OtQ CQq 

Cq;6 

d§ Cq, 6 

0 

0 

0 

1 


Thus, the last two columns on the right-hand side of equation (3.43) are 
independent of 9q. 

Step 2. Equate the third and fourth column of the left-hand side and 
right hand side of equation (3.43). This yields the following six equations 
in Oi, i = 1,..., 5: 

EQ1 : C 3/1 + S 3/2 = c 2 h\ + s 2 h 2 — «2 

EQ2 : S 3/1 — C 3/2 = — X 2 (s 2 h\ — c 2 h 2 ) + 1+^3 — d 2 ) 

EQ3 : /3 = H 2 {s 2 h\ — c 2 h 2 ) + X 2 (h ,3 — d 2 ) 

(3.45) 

EQ4 : c 3 ri + s 3 r 2 = c 2 ni + s 2 n 2 

EQ5 : s 3 ri - c 3 r 3 = -A 2 (s 2 ni - c 2 n 2 ) + P 2 n 3 

EQ 6 : r 3 = /x 3 (s 2 ni - c 2 n 2 ) + A 2 n 3 , 


where 

fi =0491+3492+03 

r\ = C 477 T 1 +547722 
9 l =c5«5+a4 
mi =s 5 fi 5 
h 1 =cip+s 1 q-a 1 

77.1 = C\U-\-SiV 


h =9393 - A3 (3491 - C492) 

02 =937113-A3 (347711—041712) 

92 =94<i5-A4S 5 a 5 

012 = 05 A495 +94 A5 
h 2 = fii(r-d 1 )-\ 1 (s 1 p-ciq) 

712 =9lU7 —Al(3lU—Oil)) 


/3 = 93 (3491 - C492) + A393 +d3 

03 =93(347711— 04777.2 )+A377l3 
93 = — S594a5+A4d5+ci4 
7713 = —059495 + A4A5 
(13 =9l(3ip —ci9) + Ai(r —di) 
713 = 91 (3111 — citi) + Aiio. 


The scalars oq and di are the Denavit-Hartenberg parameters of the links, 
and p,i = sina^, A^ = coscq. The desired configuration enters through 
the following parameters: 


p = -l x a 6 - (m x p 6 + n x X e )d 6 + q x 
q = ~l y a 6 - (rriyUe + n y \ 6 )d 6 + q v 
r = -l z a e - (m,/i 6 + n z X 6 )d 6 + q z 


u = m x fiQ + n x A 6 
v = m y p , 6 + n v Xe 
w = m z ne + n z X 6 . 


Recall that the l = (l x ,l y ,l z ), m = (m x ,m y ,m z ), n = ( n x ,n y ,n z ), and 
q — {dx, q y ,q z ) were defined in equation (3.44). 
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Step 3. Now, rearrange equations EQ1-EQ6, with the obvious definitions 
for h, f,n,r£ R 3 from the above definitions for their components, to get 
two sets of three equations: 



C2 

Si 

O' 


'1 

0 

O' 


C 3 

S3 

o' 

V = 

-si 

Cl 

0 

h = 

0 

—A 2 

M2 


S3 

-C 3 

0 


0 

0 

1 


0 

M2 

A 2 _ 


0 

0 

1 


C2 

S2 

o' 


'1 

0 

O' 


C 3 

S3 

O' 

l = 

-si 

Cl 

0 

n - 

0 

—A 2 

M2 


S 3 

-C 3 

0 


0 

0 

1 


0 

M2 

A 2 _ 


0 

0 

1 


(3.46) 

The left-hand sides of p and l are linear combinations of 1, C 2 , S 2 ,Ci, 
si, C 1 C 2 , S 1 C 2 , and S 1 S 2 . The right-hand sides are linear combinations in 
functions of S3, C3 of 1, C5, S5, C4, S4, C4C5, S4C5, and S4S5. 


Step 4- From a lengthy calculation, using the kinematics, it follows that 


l p p-p p-l pxl {p ■ p)l — 2{p ■ l)p 


have the same functional form as p and l in terms of their dependence 
on linear combinations of the same variables as the left- and right-hand 
sides of p and l. Between them, these represent 3+3+1+1+3+3 = 14 
equations. Combine these 14 equations to get an equation of the form 


P(s 3 ,c 3 ) 


1- S4S5 
S4C5 
C4 S5 
C4C5 
S 4 
C 4 
S 5 
C 5 


Q 


r S1S2 -1 
sic 2 
cis 2 
cic 2 
si 
Cl 

s 2 

L c 2 J 


(3.47) 


In this equation, P(s 3 ,c 3 ) £ R 14x9 is a function of S 3 ,C 3 and Q £ R 14x 
is a constant matrix. 


Step 5. If the rank of the matrix Q £ R 14xS is 8, use any 8 of the 14 
equations in equation (3.47) to solve for the eight variables 


S1S2 SlC2 C1S2 C1C2 Si Cl s 2 C2 


in terms of the variables 


S4S5 S4C5 C4S5 C4C5 S4 C4 S5 C5. 


Use this in the remaining six equations of (3.47) to get six equations of 
the form 


S(c 3 ,s 3 ) 


-S4S5 - 

S4C5 

C4S5 
C4C5 
S 4 

C 4 

S 5 
C 5 

L 1 J 


= 0, 


(3.48) 
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where £( 03 , 53 ) € R 6x9 . 

If the rank of Q is an integer k less than 8 , then one can only solve 
for k of the variables S 1 S 2 , S 1 C 2 , C 1 S 2 , si, c\, S 2 , C 2 and we are left with six 
equations in 9 + 8 — k variables in place of (3.48). We will not describe 
the algorithm in detail for this scenario, for details we refer the reader 
to [64], 

Step 6. We will assume that Q has rank 8 , so that the equation of (3.48) 
holds. To make the equations (3.48) polynomial rather than trigonomet¬ 
ric, use the substitutions 

2 Xi 1 — xf 


for i = 3,4, 5, where 

Xi = tan(^). 


Use this in equation (3.48) and multiply each equation by (1 + £ 4 ) and 
(l + £§) to clear the denominators. Now, multiply the first four equations 
by (1 + xj) to get 

r_ 2 2-, 

x A x 5 

X4X5 


£iOe 3 ) 


X 4 X 5 

X4X5 


OC5 

1 


= 0 . 


(3.49) 


The first four equations of equation (3.49) are quadratic in £3 and the 
last two are rational functions of £ 3 , with denominators 1 +x\. However, 
the determinant of any 6 x 6 submatrix of £ is a polynomial in £ 3 . (This 
last fact needs proof.) 


Step 7. We now use dialytic elimination to eliminate X 4 ,x$ as in the 
previous example by multiplying the six equations in equation (3.49) by 
£4 and then appending them to the original set to get 12 equations of the 
form 

1 - 3 2-i 

x 4 x 5 

X4X5 


£i(£ 3 ) 0 

0 £1 (£3) 


X 4 X 5 


X4X5 

X4X5 

X4 


= 0 . 


(3.50) 


x 5 

L 1 J 


The matrix £i(£ 3 ) is in R 6x9 and the zero matrices are of dimension 6 
x 3 so that the overall matrix on the left-hand side of equation (3.50) is 
in R 12x12 . 
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Step 8. The polynomial equation det E = 0 is a polynomial of order 16 
in 23. Once we solve for 23, we solve for 24,25 from the determination of 
the null space of E as in the Example 3.4. Then, we solve for 9 2 from 
equation (3.47). This yields the solutions to all the angles except 9q. For 
this purpose, we return to the form of the kinematics 

9st{0) = e^ lBl ■ ■ ■ e^ 9e g st {0 ) = g d , 

with all the variables known except 0 6 . By choosing any point p, we may 
write this equation as 

= e ■ ■ ■ e~^ 9l g d g~tp =: q 

Now we use Subproblem 1 from the preceding subsection to solve for 9 e . 

Number of inverse kinematics solutions 

The set of steps outlined above results in a 16th order polynomial for 23 
and hence #3. This polynomial may or may not have 16 real roots. In the 
instance of the elbow manipulator, we saw that the maximum number 
of feasible solutions was eight. One may verify that if the three wrist 
axes of the elbow manipulator do not intersect at a point, the number of 
inverse kinematics solutions for this manipulator may be as high as 16. 
Indeed, 16 is the upper bound of number of solutions of any open-link 
spatial mechanism with six degrees of freedom. This number is a sharp 
bound—that is to say, it is achievable (see, for example, [65])—and is far 
superior to those obtained from a brute force elimination technique using 
Bezout’s theorem. Indeed, this bound of 16 is a tribute to the ingenuity of 
Lee and Liang [57] who were the first to notice the set of tricks presented 
above and put to rest a number of erroneous conjectures that had been 
populating the literature up to that point. Not the least of their tricks is 
the rewriting of the kinematic equations as equation (3.43). 

The algorithm as stated above has some drawbacks, as pointed out 
by Manocha and Canny [64]. The majority of the drawbacks are numeri¬ 
cal, owing to the computation of determinants of ill-conditioned matrices, 
though there are some conceptual ones as well. For an example of a con¬ 
ceptual drawback, we have the modification of the procedure in Step 5 
above for choosing fewer than eight independent equations out of 14 to 
eliminate as many of the cosines and sines of 9\ , 9 2 when the rank of the 
matrix Q is less than 8 to produce an over determined set of equations. 
The aforementioned paper shows that a general computational technique 
of converting the dialytical elimination technique into a generalized eigen¬ 
value problem can be brought to bear on the inverse kinematics problem 
so as to improve its numerical conditioning and speed of computation. 
These computational details are of tremendous importance when real¬ 
time computation of the inverse kinematics solutions needs to be done in 
the context of obstacle avoidance for robots in cluttered environments. 
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4 The Manipulator Jacobian 

In addition to the relationship between the joint angles and the end- 
effector configuration, one often makes use of the relationship between 
the joint and end-effector velocities. In this section, we derive a formula 
for this relationship and study its structure and properties. We also study 
the dual relationship between end-effector wrenches and joint torques. 

Traditionally, one describes the Jacobian for a manipulator by dif¬ 
ferentiating the forward kinematics map. This works when the forward 
kinematics is represented as a mapping g : R” —> R p , in which case the 
Jacobian is the linear map g|(0) : 1" —> R p . However, if we represent 
the forward kinematics more completely as g : R” —► SE(3), the Jacobian 
is not so easily obtained. The problem is that §f(0) is not a natural 
quantity since g is a matrix-valued function. Of course, one could al¬ 
ways choose coordinates for SE( 3), but this description only holds locally. 
More importantly, choosing a local parameterization for SE( 3) destroys 
the natural geometric structure of rigid body motion. 

To correct this problem, we use the tools of Chapter 2 to write the 
Jacobian of the forward kinematics map in terms of twists. We shall 
see that the product of exponentials formula leads to a very natural and 
explicit description of the manipulator Jacobian, which highlights the 
geometry of the mechanism and has none of the drawbacks of a local 
representation. 

4.1 End-effector velocity 

Let g s t : Q —► SE{ 3) be the forward kinematics map for a manipulator. 
If the joints move along a path 6(t) G Q, then the end-effector traverses 
a path g s t(0{t)) £ SE( 3). The instantaneous spatial velocity of the end- 
effector is given by the twist 

E/ t = 9st{Q)g^t{0)- 


Applying the chain rule, 


A = E 


2=1 


dg s t 

dOi 




2=1 


9 gst —lfn\ 

w g - m 


(3.51) 


and we see that the end-effector velocity is linearly related to the velocity 
of the individual joints. In twist coordinates, equation (3.51) can be 
written as 


v/ t = j s s t m 


where 


J s st(0) = 


dg s t -i 
80! 9st 


dg s t _i 
dd n 9st 


V“| 


(3.52) 
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We call the matrix Jj* t (0) el 6x " the spatial manipulator Jacobian. At 
each configuration 9, it maps the joint velocity vector into the corre¬ 
sponding velocity of the end-effector. 

If we represent the forward kinematics using the product of exponen¬ 
tials formula, we can obtain a more explicit and elegant formula for 
Let 

9 st (9) = e^ 1 • • • e^ n9n g st ( 0) 

represent the mapping g s t '■ Q —> SE(3), where £, £ se(3) are unit twists. 
Then, 


dg S i 

86, 


and, converting to twist coordinates, 


9? = ^ ■ 

••«*-*-* (•**)• 

= e^ 1 ■ 


= e^ ldl ■ 

.. e £i-i0i-i (£.) e -£<-i0i 


e 6 . 0 » 5gt ( O ) gj 


— 1 


• e 


—£i#i 


dg st -i 

86 , 9st 


= Ad 


(e« 




The spatial manipulator Jacobian becomes 


■W)=[£i £2 ••• C] 

- 1 ) 


& “ Ad (/ iei ... e**- 10 *- 1 ' ^ 


(3.53) 


(3.54) 


Jg t (0) : R" —> R 6 is a configuration-dependent matrix which maps joint 
velocities to end-effector velocities. 

Equation (3.54) shows that the manipulator Jacobian has a very spe¬ 
cial structure. By virtue of the definition of £', the ith column of the 
Jacobian depends only on 6 1 ,...i. In other words, the contribution 
of the ith joint velocity to the end-effector velocity is independent of the 
configuration of later joints in the chain. Furthermore, the ith column of 
the Jacobian, 

= Ad ( e €iSi _ _ _ 

corresponds to the ith joint twist, £j, transformed by the rigid trans¬ 
formation exp(^i0i) • • • exp(£i_i$i_i). This is precisely the rigid body 
transformation which takes the ith joint frame from its reference con¬ 
figuration to the current configuration of the manipulator. Thus, the 
ith column of the spatial Jacobian is the ith joint twist, transformed to 
the current manipulator configuration. This powerful structural property 
means that we can calculate J| t (0) “by inspection,” as we shall see shortly 
in an example. 
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It is also possible to define a body manipulator Jacobian , J b tl which is 
defined by the relationship 


v? t = 

A calculation similar to that performed previously yields 


■&(*) = & ••• d-i etl 

4= Ad; 1 - - f, 

(e^ e ' ■■■e^g st ( 0 )) 


(3.55) 


The columns of J b t correspond to the joint twists written with respect 
to the tool frame at the current configuration. Note that g st ( 0) appears 
explicitly; choosing S such that g st { 0) = I simplifies the calculation of J b t . 
The spatial and body Jacobians are related by an adjoint transformation: 


J s st (9) = Ad gst{e) J b st (0). (3.56) 

The spatial and body manipulator Jacobians can be used to compute 
the instantaneous velocity of a point attached to the end-effector. Let 
q b represent a point attached to the end-effector, written in body (tool) 
coordinates. The velocity of q b , also in body coordinates, is given by 

v b q = V^q b =(j b s M0)\ b - 

Similarly, if we represent our point relative to the spatial (base) frame, 
then 

v s q = VsW= (J s st (0)0)% s . 

If we desire the velocity of the origin of the tool frame, then q b = 0 but 
q s = g st (9)q b = p{9 ), the position component of the forward kinematics 
map. Thus, using homogeneous coordinates explicitly, 


v 


S 

q 


'p{6) 

0 


= RstV b t 



'p(6) 

1 


are all equivalent expressions for the velocity of the origin of the tool 
frame. 

The relationship between joint velocity and end-effector velocity can 
be used to move a robot manipulator from one end-effector configuration 
to another without calculating the inverse kinematics for the manipulator. 
If J s t is invertible, then we can write 

m = (3.57) 
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Figure 3.13: SCARA manipulator in non-reference configuration. 

If Vf t (t) is known, equation (3.57) is an ordinary differential equation for 
8 . To move the end-effector between two configurations g\ and 32 , we 
pick any workspace path g(t) with g( 0) = g± and g(T) = 32 , calculate 
the spatial velocity V s = gg _1 , and integrate equation (3.57) over the 
interval [0,T]. 

Example 3.8. Jacobian for a SCARA robot 

Consider the SCARA robot at an arbitrary configuration 8 £ Q, as shown 
in Figure 3.13. The spatial Jacobian can be evaluated by writing the 
twists associated with each joint in its current configuration. For the 
SCARA, the directions of the twists are fixed and only the points through 
which the axes of the twists pass are functions of 8 . By inspection, 



'O' 


— 1 \ sin 8 \ 


—li sin 8 \ — I 2 sin(0i + 8 2 ) 

Qi = 

0 

<?2 = 

l\ cos 8 1 

<73 = 

l\ cos + I 2 cos(0i + 82 ) 


0 


0 


0 


are points on the axes. Calculating the associated twists yields 

l\ cos 8 \ -(-12 cos($i A 82 ) 0 

l\ sin 8\ + l 2 sin($i + d 2 ) 0 

0 1 

0 0 ' 

0 0 

1 0 

As a check, one can calculate the linear velocity of the end-effector using 
the formula (J| t 0) A p(0) and verify that it agrees with p( 8 ). 


7 s = 

J St ~ 


li cos 8 1 
1 1 sin 8 \ 
0 
0 
0 
1 
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Figure 3.14: An idealized version of the Stanford arm. 


Example 3.9. Jacobian for the Stanford arm 

The Stanford arm, shown in Figure 3.14, is a six degree of freedom robot 
with two revolute joints at the base, a prismatic joint, and a spherical 
wrist. It is very similar to an elbow manipulator, with the “elbow” re¬ 
placed by a prismatic joint. 

The spatial manipulator Jacobian for the Stanford arm is computed by 
determining the location and directions of the joint twists as a function 
of the joint angles. For example, the first two joints pass through the 
point qi = q2 = ( 0 , 0 , Zo) and point in the directions u >i = ( 0 , 0 , 1 ) and 
u >2 = (— cos 0 1 , — sin 9i , 0). This gives joint twists 




"O' 




Iq sin 0 1 

-Wl x q 1 


0 

0 

= 

—w 2 X qi 


— Zo cos 0 1 

0 

0Ji 


0 

0 

S2 

W 2 


— cos Q\ 

— sin 0 1 



. 1. 




0 


The third joint is prismatic and hence we care only about its direction. 
Taking into account the change in orientation due to the first two joints, 
we have 





■ — sin 0i cos 6 2 - 


gZ0i g—X0 2 

0 

1 


COS 01 COS 02 


Vo 


. 0 . 

— 

— sin 02 

— 

o 

0 



0 

0 


0 




L o J 



Finally, we compute the twists corresponding to the wrist. The wrist is 
located at the point 


'O' 

+ e* 9 l e ~* 92 

0 


— 0i +# 3 ) sin 61 cos 02 

0 

h + O 3 

= 

01 + 03 ) COS 01 COS 02 

Jo. 


0 


^0 — 0i + $ 3 ) sin 0 2 
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The directions of the wrist axes depend on 9\ and 62 as well as the 
preceding wrist axes. These are given by 


w' A = e 2 Sl e - Xfi2 


'o' 


-S1S2 

0 

= 

C1S2 

1 


c 2 


<J B = e^e-^e* 9 * 


‘-1 


~CiC4 + S1C2S4 

0 

= 

— S1C4 — C1C2S4 

0 


S2S4 


. = e 2Sl e -26 * 2 e 2 ® 4 e -x05 

'O' 


~ Cs(siC2C4 + C1S4) + S1S2S5 

1 

= 

C5(ciC2C4 — S1S4)— C1S2S5 


0 


— S2C4C5 — C2S5 


Combining these directions with our calculations for q' w , we can now write 
the complete manipulator Jacobian: 


rs _ [ 0 -w ' 2 x qi v ' 3 -w. 4 x q' w -u/ 5 x q' w ~u' 6 x q' w 

st U >1 (Jj ' 2 0 W 4 u >' 5 lo ' 6 

(3.58) 

where the various quantities are defined above. 

Note that we were able to calculate the entire manipulator Jacobian 
without explicitly differentiating the forward kinematics map. 

As a final comment, we re-emphasize that the manipulator Jacobian 
differs from the Jacobian of a mapping / : R” —> R p . For instance, 
in Example 3.8 (the SCARA manipulator), it is possible to define the 
configuration of the end-effector as x = (p( 8 ), <j>( 0 )), where p( 8 ) is the 
xyz position of the tool frame and (f>{ 8 ) = 81 + 82+83 is the angle 
of rotation of the tool frame about the 2 -axis. The kinematics is then 
represented by the mapping x = f ( 8 ) and by the chain rule 



The matrix is the Jacobian of the mapping / : Q —> R 4 , but it is not 
the manipulator Jacobian (body or spatial). In particular, the columns of 
^ cannot be interpreted as the instantaneous twist axes corresponding 
to each joint. 

Similarly, for a general manipulator, one can choose a local parame¬ 
terization for SE(3) and write the kinematics as / : Q —■<■ R 6 . Once again, 
the Jacobian of the mapping / : Q —> R 6 has no direct geometric inter¬ 
pretation, even though it has the same dimensions as the manipulator 
Jacobian. Furthermore, for manipulators which generate full rotation of 
the end-effector, the parameterization of SE(3) by a vector of six numbers 
introduces singularities which are solely an artifact of the parameteriza¬ 
tion. These singularities may lead to false conclusions about the ability 
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of the manipulator to reach certain configurations or achieve certain ve¬ 
locities. The use of the manipulator Jacobian, as we have defined it here, 
avoids these difficulties. 

4.2 End-effector forces 

The manipulator Jacobian can also be used to describe the relationship 
between wrenches applied at the end-effector and joint torques. This 
relationship is fundamental in understanding how to program robots to 
interact with their environment by application of forces. We shall see 
that the duality of wrenches and twists discussed in Chapter 2 extends 
to manipulator kinematics. 

To derive the relationship between wrenches and torques, we calculate 
the work associated with applying a wrench through a displacement of the 
end-effector. If we let g s t{9(t)) represent the motion of the end-effector, 
the net work performed by applying a (body) wrench F t over an interval 
of time [ti, £ 2 ] is 

W= f 2 V s \- F t dt, 

Jt 1 

where V^ t is the body velocity of the end-effector. The work will be the 
same as that performed by the joints (assuming no friction), and hence 

f 2 6 -Tdt = W = r V s b t -F t dt. 

Since this relationship must hold for any choice of time interval, the 
integrands must be equal. Using the manipulator Jacobian to relate U s b t 
to 6 , we have 

9 T T = 9 T (J b st fF t . 

It follows that since 9 is free, 


r = ( J b st ) T F t . (3.59) 

This equation relates the end-effector wrench to the joint torques by 
giving the torques that are equivalent to a (body) wrench applied at the 
end-effector. 

A similar analysis can be used to derive the relationship between a 
spatial wrench F s applied at the end-effector and the corresponding joint 
torques: 

r=(J s st ) T F s . (3.60) 

The full derivation of this equation is left as an exercise. 

The interpretation of the Jacobian transpose as a mapping from end- 
effector forces to joint torques must be made carefully. If the Jacobian 
is square and full rank, there are no difficulties. However, in all other 
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cases, the relationship can be misleading. We defer the discussion of 
singularities to the next section, and consider only the case when J st is 
not square. 

The formulas given by equations (3.59) and (3.60) describe the force 
relationship that must hold between the end-effector forces and joint 
torques. We can use these equations to ask two separate questions: 


1 . If we apply an end-effector force, what joint torques are required to 
resist that force? 

2. If we apply a set of joint torques, what is the resulting end-effector 
wrench (assuming that the wrench is resisted by some external 
agent)? 

Equation (3.59) answers the first question in all cases. However, in ma¬ 
nipulation tasks, we are often more interested in answering the second 
question, which can be recast as: what joint torques must be applied to 
generate a given end-effector wrench? 

If the number of joints is larger than the dimension of the workspace, 
then we say the manipulator is kinematically redundant. In this case, 
we can generically find a vector of joint torques which generates the ap¬ 
propriate end-effector force, as given by equation (3.59). However, since 
there are more joints than the minimum number required, internal mo¬ 
tions may exist which allow the manipulator to move while keeping the 
position of the end-effector fixed. Redundant manipulators are discussed 
in more detail in Section 5. 

If, on the other hand, the number of joints is smaller than the di¬ 
mension of the workspace, then there may be no torque which satisfies 
equation (3.59) for arbitrary end-effector wrenches, and therefore some 
end-effector wrenches cannot be applied. They can, however, be resisted 
by the manipulator. This is a consequence of our assumption that the 
allowable motion of the manipulator is completely parameterized by the 
joint angles 9. If a wrench causes no joint torques, it must be resisted by 
structural forces generated by the mechanism. Such a situation occurs 
when F lies in the null space of Jj t . In this case, the force balance equa¬ 
tion is satisfied with r = 0; the resisting forces are supplied completely 
by the robot’s mechanical structure. 


Example 3.10. End-effector forces for a SCARA robot 

Consider again the SCARA robot from the Example 3.8. The transpose 
of the spatial manipulator Jacobian is 


m 


T 


0 0 0 0 0 

ZiCi Zi-Si 0 0 0 

l\C\ + I2C12 l\S\ + I2S12 0 0 0 

0 0 10 0 


1 

1 

1 

0 
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Figure 3.15: End-effector wrenches which generate no joint torques. 
The null space of this matrix is spanned by 


"O' 


"0" 

0 


0 

0 

1 

f n 2 = 

0 

0 

0 


1 

LoJ 


_ 0 _ 


hence, workspace torques about the x- and y -axes of the manipulator 
cannot be applied by the manipulator. For example, twisting the manip¬ 
ulator as shown in Figure 3.15 generates no joint torques and no motion 
of the manipulator. 


4.3 Singularities 

At a given configuration, the manipulator Jacobian describes the rela¬ 
tionship between the instantaneous velocity of the end-effector and the 
joint velocities: 

v b s t = J&m 

A singular configuration of a robot manipulator is a configuration at 
which the manipulator Jacobian drops rank. For a six degree of freedom 
manipulator in SE( 3), the Jacobian fails to be invertible at singular points 
and hence the manipulator is not able to achieve instantaneous motion 
in certain directions. Near singular configurations, the size of the joint 
velocities required to maintain a desired end-effector velocity in certain 
directions can be extremely large. 

If a manipulator has fewer than six degrees of freedom, a singular con¬ 
figuration corresponds to a configuration in which the number of degrees 
of freedom of the end-effector drops. This is again characterized by the 
manipulator Jacobian dropping rank, i.e., two or more of the columns 
of J| t (0) £ R 6xra become linearly dependent. Since most manipulators 
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are designed for tasks in which all of the degrees of freedom are needed, 
singular configurations should usually be avoided, if possible. 

Singularities also affect the size of the end-effector forces that the 
manipulator can apply. At a singular configuration, some end-effector 
wrenches will lie in the null space of the Jacobian transpose. These 
wrenches can be balanced without applying any joint torques, as the 
mechanism will generate the opposing wrenches. On the other hand, 
applying an end-effector wrench in a singular direction is not possible. 
Such a wrench could be balanced by any wrench in the singular direction, 
and hence it can be balanced by the zero wrench. If no other external 
forces are present, no forces will be generated. 

In order to avoid these difficulties, it is necessary to identify singular 
configurations of a manipulator. We concentrate on classifying several 
common singularities for six degree of freedom manipulators and show 
how these can be determined by analyzing the geometry of the system. 
The cases presented here can be extended to consider more general open- 
chain manipulators. For each of the geometric conditions given below, 
we give a sketch of the proof of singularity. To illustrate some of the 
different ways in which singularities can be analyzed, we use a different 
proof technique for each example. 

Example 3.11. Two collinear revolute joints 

The Jacobian for a six degree of freedom manipulator is singular if there 
exist two revolute joints with twists 

- 0 J 2 x q 2 

U ) 2 

which satisfy the following conditions: 

1. The axes are parallel: u>\ = ±u> 2 . 

2. The axes are collinear: u>i x (qi — q 2 ) = 0, * = 1, 2. 

Proof. In analyzing the singularity of a matrix, we are permitted to pre- 
or post-multiply the matrix by a nonsingular matrix of the proper dimen¬ 
sions. Pre-multiplication by a nonsingular matrix can be used to add one 
row to another or switch two rows, while post-multiplication can be used 
to perform the same operations on columns. 

Assume, without loss of generality, that the columns of the Jacobian 
which are linearly dependent are the first two columns of J. The Jacobian 
has the form 


6 = 


-w 1 x q ± 

Ui 


m 


-uq x qi ~u> 2 x q 2 

UJl UJ 2 



6 x6 
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and we can assume u>\ = u 2 by negating the second column if necessary. 
Subtracting column 1 from column 2 yields 


m 


-LU! X qi X (q 2 - qi) 

uj\ 0 


where the symbol ~ denotes equivalence of two matrices (up to elemen¬ 
tary column operations). Using condition 2, the second column is zero, 
so that J{6) is singular. □ 

This type of singularity is common in spherical wrist assemblies that 
are composed of three mutually orthogonal revolute joints whose axes 
intersect at a point. By rotating the second joint in the wrist, it is 
possible to align the first and third axes and the manipulator Jacobian 
becomes singular. In this configuration, rotation about the axis normal 
to the plane defined by the first and second joints is not possible. 

Example 3.12. Three parallel coplanar revolute joint axes 

The Jacobian for a six degree of freedom manipulator is singular if there 
exist three revolute joints which satisfy the following conditions: 

1. The axes are parallel: u>i = ±ujj for i, j = 1, 2, 3. 

2. The axes are coplanar: there exists a plane with unit normal n such 
that n T u)i = 0 and 


n T (qi~qj) = 0, i,j = 1,2,3. 


Proof. Another type of transformation which can be used in analyzing 
singularities is to change the frame of reference used to express the twists 
that form the columns of the Jacobian. A change of coordinates affects 
twists (and hence the Jacobian) by pre-multiplying by the adjoint matrix 
corresponding to the change of basis. Since the adjoint is an invertible 
transformation (Ad^ 1 = Ad ff -i), this does not affect the singularity of 
the matrix. 

After an initial column permutation, assume J(9) has the form 


m 


-LOi x qi -U2 X q 2 -u >3 x q 3 
UJl UJ 2 W 3 


Attach a coordinate frame to the point qi with the 2 -axis of the frame 
pointing in the direction of u>i (see Figure 3.16). Further, choose the 
frame such that the plane formed by the axes is the yz plane in the new 
coordinates. Thus, each axis has a point of intersection which lies on the 
y-axis. Call these points yi{= 0), y 2 , and y 3 . Now, with respect to this 
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Figure 3.16: Three coplanar, parallel, revolute twists. 


frame, the Jacobian has the form 

± 2/2 ± 2/3 

0 0 ••• 

0 0 

0 0 

0 0 ••• 

±1 ±1 

The first three columns are clearly linearly dependent. □ 

The elbow manipulator exhibits this singularity in its reference con¬ 
figuration (see Figure 3.11). 

Example 3.13. Four intersecting revolute joint axes 

The Jacobian for a six degree of freedom manipulator is singular if there 
exist four revolute joint axes that intersect at a point q: 

Ui x fe - q) = 0, i = 1,... ,4. 

Proof. This example is trivial if we choose a frame whose origin is at the 
common point of intersection of the four revolute twists. However, we can 
also show singularity by making use of reciprocal screw systems. Recall 
from Section 5.3 of Chapter 2 that a wrench is reciprocal to a twist when 
the inner product between the wrench and the twist is zero (indicating 
that no work is done by applying the wrench and moving along the twist). 
Since we are in a 6-dimensional space, if we can show that the dimension 
of the reciprocal system is sufficiently large (three for this example), then 
we can show singularity of the system of twists. This technique works 
well when there are a large number of twists and hence the size of the 
reciprocal system is small. 

For this example, we make use of the following fact: every revolute 
twist is reciprocal to a pure force, in any direction, applied to a point on 


Ad g J(0) = 
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the axis of the revolute twist. To see this, it suffices to consider a twist 
and wrench through the origin: 

V = [° 

u> 

It is left to the reader to verify that this case generalizes appropriately. 

We can now use this fact to construct the reciprocal system for the 
four twists which intersect at a point. Since any pure force through this 
point corresponds to a reciprocal wrench, it follows that the dimension of 
the reciprocal system is three and hence the four twists must be singular. 

□ 

This type of singularity occurs in the inverse elbow manipulator (see 
Exercise 4) when the final joint axis intersects the shoulder adding a 
fourth axis as shown. 

The singularities given here and in the exercises are by no means 
exhaustive. However, they do occur frequently and are often easy to de¬ 
termine just by examining the geometry of the manipulator. It is also 
possible for a manipulator to exhibit different types of singularities at a 
single configuration. In this case, depending on the number and type of 
the singularities, the manipulator may lose the ability to move in several 
different directions at once. For example, if the arm of the elbow manip¬ 
ulator shown in Figure 3.4 is held vertically over the base, it exhibits all 
three of the singularities we have just illustrated. However, it still has 
four degrees of freedom (instead of three) since two of the singularities 
restrict motion in the same direction. 

In addition to singularities of the manipulator Jacobian, a robot can 
also lose degrees of freedom when the joint variables are constrained to 
he in a closed interval. In this case, a loss of freedom of motion can occur 
when one or more of the joints is at the limit of its travel. At such a 
configuration, motion past the joint limit is not allowed and the motion 
of the end-effector is restricted. 

4.4 Manipulability 

As we saw in the previous section, when a manipulator is at a singular 
configuration there are directions of movement which require high joint 
rates and forces. Near a singularity, movement may also be difficult in 
certain directions. The manipulability of a robot describes its ability to 
move freely in all directions in the workspace. 

Manipulability measures can be divided into two rough classes: 

1. The ability to reach a certain position or set of positions 

2. The ability to change the position or orientation at a given config¬ 
uration 


F = 


V t F = 0. 
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The first of these measures is directly related to the workspace of a ma¬ 
nipulator. Depending on the task, we may want to use the complete, 
reachable, or dextrous workspaces to characterize the manipulability of 
a manipulator. The second class of measures concerns the manipulabil¬ 
ity of a manipulator around a given configuration; that is, it is a local 
property. 

To study local manipulability, we examine the Jacobian of the manip¬ 
ulator, which relates infinitesimal joint motions to infinitesimal workspace 
motions. Throughout this section we write J for the manipulator Jaco¬ 
bian J st . Either the spatial or body Jacobian can be used, but the body 
Jacobian is preferred since the body velocity of the end-effector is inde¬ 
pendent of the choice of base frame. 

There are many different local manipulability measures that have been 
proposed in the literature and which are useful in different situations. We 
present a small sample of some of the more common measures here. Many 
of these measures rely on the singular values of J. Recall that for a matrix 
A e M. pxn , the singular values of A are the square roots of the eigenvalues 
of A T A. We write cr{A) for the set of singular values of A and A(A) to 
denote the set of eigenvalues of A. The maximum singular value of a 
matrix is equal to the induced two-norm of the matrix: 

CTmax(A) = max \\Axh = \\ A h- 
IM|2=i 

If a matrix is singular, then at least one of its singular values is zero. 

Example 3.14. Minimum singular value of J 


Hl(9) = 


The minimum singular value of the Jacobian corresponds to the minimum 
workspace velocity that can be produced by a unit joint velocity vector. 
The corresponding eigenvector gives the direction (twist) in which the 
motion of the end-effector is most limited. At a singular configuration, 
the minimum singular value of J is zero. 

Example 3.15. Inverse of the condition number of J 


M 0 ) 


CTmax (J(0)) 


The condition number of a matrix A is defined as the ratio of the max¬ 
imum singular value of A to the minimum singular value of A. For the 
Jacobian, the inverse condition number gives a measure of the sensitivity 
of the magnitude of the end-effector velocity V to the direction of the 
joint velocity vector 9. It provides a normalized measure of the minimum 
singular value of J. At a singular configuration, the inverse condition 
number is zero. 
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Example 3.16. Determinant of J 

ii3(9) = det J(9) 

The determinant of the Jacobian measures the volume of the velocity 
ellipsoid (in the workspace) generated by unit joint velocity vectors. It 
is important to note that ^ 3 ( 6 ) does not contain information about the 
condition number of J. In particular, since det J(9) is the product of 
the singular values of J(0), it can be large even if cr m i n (J(9)) is small, by 
having a large a max (J(9)). 

These manipulability measures can be used to provide an alternate 
definition for the dextrous workspace of a manipulator. For any of the 
measures given above, define the set W' D as 

W' D = { g st (9 ) : 9 G Q, m(9) ± 0} C SE( 3). (3.61) 

W' n is the set of end-effector configurations for which the manipulator can 
move infinitesimally in any direction. Note that W' D is a subset of SE( 3), 
unlike our previous definition (in equation (3.13)) which consisted of the 
subset of R 3 at which the manipulator could achieve any orientation. 
Additional manipulability measures are given in the exercises. 


5 Redundant and Parallel Manipulators 

In this section, we briefly consider some other kinematic mechanisms that 
occur frequently in robotic manipulation. We focus on two particular 
types of structures—redundant manipulators and parallel manipulators— 
and indicate how to extend some of the results of this chapter to cover 
these cases. 

5.1 Redundant manipulators 

In order to perform a given task, a robot must have enough degrees of 
freedom to accomplish that task. In the analysis presented so far, we have 
concentrated on the case in which the robot has precisely the required 
degrees of freedom. A kinematically redundant manipulator has more 
than the minimal number of degrees of freedom required to complete a 
set of tasks. 

A redundant manipulator can have an infinite number of joint config¬ 
urations which give the same end-effector configuration. The extra de¬ 
grees of freedom present in redundant manipulators can be used to avoid 
obstacles and kinematic singularities or to optimize the motion of the 
manipulator relative to a cost function. Additionally, if joint limits are 
present, redundant manipulators can be used to increase the workspace 
of the manipulator. 
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The derivation of the forward kinematics of a redundant manipulator 
is no different from the derivation presented in Section 2. Using the 
product of exponentials formula, 


where n is greater than p 7 the dimension of the workspace (p = 3 for 
planar manipulators and p = 6 for spatial manipulators). The Jacobian 
of a redundant manipulator has the form 

■W)=[& & ••• &], 

where £' is the twist corresponding to the ith joint axis in the current 
configuration. Jf t € R px " has more columns than rows. 

The inverse kinematics problem for a redundant manipulator is ill- 
posed: there may exist infinitely many configurations of the robot which 
give the desired end-effector configuration. In fact, if we keep the end- 
effector configuration fixed, the robot is still free to move along any tra¬ 
jectory which satisfies 

9st{0{t)) = g d , (3.62) 

where gd £ SE( 3) is the desired configuration of the end-effector. The set 
of all 9 which satisfy this equation is called the self-motion manifold for 
the configuration gd . Differentiating equation (3.62), we obtain 

J S sMt))d = (gstg^r = 0 . 

Thus, the motions which are allowed must have joint velocities which 
lie in the null space of the manipulator Jacobian. A motion along the 
self-motion manifold is called an internal motion. 

More generally, given an end-effector path g(t ), we would like to find a 
corresponding joint trajectory 9{t). Since there may be an infinite number 
of joint trajectories which give the requisite end-effector path, additional 
criteria are used to choose among them. One common solution is to 
choose the minimum joint velocity which gives the desired workspace 
velocity. This is achieved by choosing 

9 = Jt t (9)Vsu 

where = J T (JJ T ) _1 is the Moore-Penrose generalized inverse of J. 
The properties of this and other kinematic redundancy resolution algo¬ 
rithms are discussed briefly in Chapter 7. 

The manipulator Jacobian can also be used to relate joint torques 
to end-effector wrenches for redundant manipulators. Since the links of 
the manipulator are free to move even when the end-effector is fixed, a 
thorough understanding of the relationship between joint forces and end- 
effector wrenches requires a study of the dynamics of the manipulator. 
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Figure 3.17: Self-motion manifold for a redundant planar manipulator. 

In particular, the possible existence of internal motions, combined with 
the inertial coupling between the links, can cause forces to be applied to 
the end-effector even if no joint torques are applied. We defer a com¬ 
plete discussion of this situation until Chapter 6, in which we study the 
dynamics of constrained systems in full detail. Using the results of that 
chapter, it will be possible to show that when a manipulator is in static 
equilibrium , the previous relationship, 



(3.63) 


still holds. This relationship gives the joint torques necessary to produce a 
given end-effector wrench when the system is stationary. Either the body 
or spatial Jacobian can be used, as long as the wrench F is represented 
appropriately. 

Example 3.17. Self-motion manifold for a planar manipulator 

Consider the planar manipulator shown in Figure 3.17a. Holding the po¬ 
sition of the end-effector fixed, the system obeys the following kinematic 
constraints: 

li cos + l 2 cos(0i + 6 2 ) + I 3 cos(0i + 0 2 + O 3 ) = x 
l\ sin 0i + I2 sin(0i + 0 2 ) + / 3 sin(0i + 02 + 63) = y. 

This is a set of two equations in three variables and hence there exist 
multiple solutions. A self-motion manifold for this manipulator is shown 
in Figure 3.17b. 

The Jacobian for the mapping p : 0 1 —> (x, y) is 

dp _ —llSl — I2S 12 — ^3^123 —^2^12 ^ I3S123 —I3S123 /o gq\ 

89 I 1 C 1 + I 2 C 12 + ^3 C 123 hci 2 + ^3 C 123 ^3 C 123 ’ 
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Figure 3.18: A parallel manipulator consisting of three series chains con¬ 
nected to a single end-effector. 


where Sijk = sin( 6 *i + Oj + 6k) and similarly for Cijk- The Jacobian has a 
null space spanned by the vector 


Vn 


hi 3 sin 6 3 

~hh sin 6*3 -hh sin( 0 2 + 0 3 ) 
lih sin 0 2 + I 1 I 3 sin( 0 2 + @ 3 ) 


Any velocity 9 = aVjy is tangent to the self-motion manifold and main¬ 
tains the position of the end-effector. One such velocity is shown as an 
arrow on Figure 3.17b. 


5.2 Parallel manipulators 

A parallel manipulator is one in which two or more series chains con¬ 
nect the end-effector to the base of the robot. An example is shown in 
Figure 3.18. Parallel manipulators can offer advantages over open-chain 
manipulators in terms of rigidity of the mechanism and placement of the 
actuators. For example, the manipulator in Figure 3.18 can be completely 
actuated by controlling only the first link in each chain, eliminating the 
need to place motors at the distal links of the manipulator. Parallel ma¬ 
nipulators are also called closed-chain manipulators, since they contain 
one or more closed kinematic chains. 

Structure equation for a parallel mechanism 

The forward kinematics for a parallel manipulator are described by equat¬ 
ing the end-effector location specified by each chain. Suppose we have a 
manipulator with n\ joints in the first chain (including the end-effector) 
and n 2 joints in the second chain (including the end-effector). Then, the 
forward kinematics is described in exponential coordinates as 

g s t = e « llSl1 • • • e^ 6l ^g st {0) = e « 2lf?21 • • • e^ e ^g st { 0), (3.65) 
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where all quantities are specified relative to a single base and tool frame. 
Equation (3.65) is called the structure equation (or loop equation) for the 
manipulator and introduces constraints between the possible joint angles 
of the manipulator. It is because of these constraints that we can control 
the end-effector location by specifying only a subset of the joint variables: 
the other joint variables must take on values such that equation (3.65) is 
satisfied. 

Since the joint variables are constrained by equation (3.65), the joint 
space for a parallel manipulator is not simply the Cartesian product of 
the individual joint spaces, as in the open-chain case. Rather, it is the 
subset Q' C Q which also satisfies equation (3.65). Determining the 
dimension of Q ', and hence the number of degrees of freedom for the 
parallel manipulator, requires careful inspection of the number of joints 
and links in the mechanism. 

Let N be the number of links in the mechanism, g the number of joints, 
and fi the number of degrees of freedom for the ith joint. The number of 
degrees of freedom can be obtained by taking the total degrees of freedom 
for all of the links and subtracting the number of constraints imposed by 
the joints attached to the links. If all of the joints define independent 
constraints, the number of degrees of freedom for the mechanism is 

9 g 

F = 6N — ^(6 — fi) = 6(N — g) + ^ /)• (3.66) 

i= 1 i=1 

Equation (3.66) is called Gruebler’s formula. Gruebler’s formula only 
holds when the constraints imposed by the joints are independent. For 
planar motions, Gruebler’s formula holds with 6 replaced by 3. 

Although the forward kinematics of a parallel manipulator is compli¬ 
cated by the closed loop nature of the mechanism, the inverse problem 
is no more difficult than in the open-chain case. Namely, the inverse 
kinematics problem for a parallel manipulator is solved by considering 
the inverse problem for each open-chain mechanism which connects the 
ground to the end-effector. This can be done using the methods presented 
in Section 3. 


Velocity and force relationships 

The velocity of the end-effector of a parallel manipulator is related to the 
velocity of the joints of the manipulator by differentiating the structure 
equation (3.65). This gives a Jacobian matrix for each chain: 


'0 n 


i 

to 

_i 


- T s 
— J 2 


l 

e 

_i 


02n 2 . 


(3.67) 
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where 


Ji — [£n £12 ''' £im] Ji — [C21 £22 



Since the mechanism contains closed kinematic chains, not all joint ve¬ 
locities can be specified independently. 

The manipulator Jacobian can be written in more conventional form 
by stacking the Jacobians for each chain: 


Ji 

0 




(3.68) 


This equation has a form very similar to one which we shall use in Chap¬ 
ter 5 to describe the kinematics of a multifingered grasp. Indeed, if we 
view each of the chains as grasping the end-effector link via a prismatic 
or revolute joint, then a parallel manipulator is very similar to a multi¬ 
fingered hand grasping an object. 

The relationship between joint torques and end-effector forces for a 
parallel manipulator is more complicated than for an open-chain manip¬ 
ulator. The basic problem is that two or more chains can fight against 
each other and apply forces which cause no net end-effector wrench. A 
set of joint torques which causes no net end-effector wrench is called an 
internal force. We defer a discussion of internal forces until Chapter 5, 
where they arise naturally in the context of grasping. 


Singularities 

Determining the singularities of parallel mechanisms is more involved 
than it is for serial mechanisms. Consider a general parallel mechanism 
with k chains and let 0,; denote the joint variables in the ith chain. The 
Jacobian of the structure equations has the form 

V s s t = = • • • = Jfe(B fc )0fc- (3-69) 

We say that an end-effector velocity is admissible at a configuration 0 = 
(0i,..., 0fc) if there exist 0, which satisfy equation (3.69). The set of 
admissible velocities forms a linear space, since it is the intersection of 
the range spaces of Ji{0), i = 1,..., k. Hence, we can define the rank of 
the structure equations , p, at a configuration 0 as the dimension of the 
space of admissible velocities, 


k 

p = dim P'1 TZ(Ji(9)), (3.70) 

;=i 

where 7 Z(A) denotes the range space of the matrix A. 

A parallel mechanism is kinematically singular at a point if the rank 
of the structure equations drops at that point. In this case, the tool loses 
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(a) Reference configuration (b) Angle description 

Figure 3.19: Four-bar linkage. 


the ability to move instantaneously in some direction. This is analogous 
to our description of singularities in serial mechanisms. However, at 
this point we have not yet identified which joints in the mechanism are 
actuated and which are passive. If a parallel mechanism is fully actuated, 
then these are the only types of singularities that can occur. However, 
in most instances, only some of the joints of a parallel manipulator are 
actuated and this can lead to additional singularities. We call this second 
type of singularity an actuator singularity and give an example of it when 
we study the Stewart platform in Section 5.4. 

5.3 Four-bar linkage 

To illustrate some of the concepts introduced above, we consider the 
four-bar linkage shown in Figure 3.19. The mechanism consists of three 
rigid bodies connected together by revolute joints. The links attached to 
the ground frame are called the input and output links, the rigid body 
which connects the input and output links is called the coupler. It is 
called a four-bar mechanism even though there are only three links, since 
historically the ground frame is considered to be the fourth link. 

Four-bar linkages are usually studied in the context of mechanism 
synthesis. For example, we might try to find a mechanism such that the 
input and output links satisfy a given functional relationship, creating 
a type of mechanical computer. Alternatively, one might wish to design 
the mechanism so that a point on the coupler traces a specified path or 
passes through a set of points. Many other variations exist, but for all 
of these problems one tries to choose the kinematic parameters which 
describe the mechanism—the lengths of the links, shape of the coupler, 
location of the joints—so that a given task is performed. In this section, 
we bypass the synthesis problem and concentrate on kinematic analysis 
of a given mechanism. 
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The number of degrees of freedom of a four-bar mechanism is given 
by Gruebler’s formula: 

N = 3 links 

g = 4 joints ==> F = 3(3 — 4) + 4 = 1. 

fi = 1 DOF/link 

The fact that there is only one degree of freedom helps explain the ter¬ 
minology of the input and output links. Note that we used the planar 
version of Gruebler’s formula to calculate the mobility of the mechanism. 
A quick calculation shows that the spatial version of the formula gives 
F = 6(3 — 4) + 4 = —2 (!). We leave the resolution of this apparent 
paradox as an exercise. 

To write the structure equations, we must first assign base and tool 
frames and choose a reference configuration. The tool and base frames 
are assigned as shown in the figure. We choose the reference configura¬ 
tion (0 = 0) to be the configuration shown in Figure 3.19a. Note that 
this is not the usual reference configuration if we had considered each of 
the kinematic chains as independent two-link robots. However, it will be 
convenient to have the reference configuration satisfy the kinematic con¬ 
straints and hence we define the angles as shown in Figure 3.19b. With 
respect to this configuration, the structure equations have the form 

9st = / lieil e« 12f,12 <? st ( 0) = / 2 i« 2 i/ 22 % st ( 0). 


Note that in the plane this gives three constraints in terms of four vari¬ 
ables, leaving one degree of freedom as expected. 

The twists can be calculated using the formulas for twists in the plane 
derived in the exercises at the end of Chapter 2. In particular, a revolute 
joint in the plane through a point q = (q x , q y ) is described by a planar 
twist 


£ = 


q y 

Qx 

1 


£ R 3 . 


This yields 
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Expanding the product of exponentials formula gives 

—r - h sin0n + r cos(0n + d 12 ) = x = r — l 2 sin0 2 i - r cos(0 2 i + ^ 22 ) 
h cos 0n + r sin(0n + 0 J2 ) = y = h + l 2 cos 0 2 i - r sin(0 2 i + 0 22 ) 

011 + 012 =(j> = 0 2 1 + 022 
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where <f> is the angle the tool frame makes with the horizontal. From the 
form of this equation, it is clear that solving for the forward kinematics 
is a complicated task, though it turns out that in many cases it can be 
done in closed form. 

The Jacobian of the structure equation has the form 


V£ = [Z H £J 2 ] 


On 

012 


[&1 



To calculate the individual columns of the Jacobian, we write the twists 
at the current configuration of the manipulator. Thus, 



'0 

l\ COS 011 


0H 

012. 


' h 

h + l 2 COS 0 2 l 


021 

022. 

II 

5? 

r 

r + 1 1 sin 0ii 
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—r 

—r + l 2 sin 0 2 i 



1 

1 



1 

1 



This gives the velocity constraints on the system. Since the individual 
Jacobians for each chain only have two columns, it is clear that the dimen¬ 
sion of the space of admissible velocities is at most two. To examine the 
mobility more closely, we rearrange the Jacobian to isolate the actuated 
and passive joints. 

Suppose we take 6 = On as the actuated joint and let a= ( 6 * 12 , 821 , 822 ) 
represent the passive joints. The Jacobian of the structure equation can 
be rearranged as 

£n0=[-&2 61 £ 22 ] « (3-71) 

(this type of rearrangement works only in the special case where we have 
two serial chains or a single kinematic loop). The form of this equation 
suggests that if we specify the velocity of the actuated joint 8 , then we 
can solve for the velocity of the passive joints if the right-hand side of 
equation (3.71) is nonsingular. 

The right-hand side of equation (3.71) corresponds to the twists gen¬ 
erated by three parallel, revolute joints. We know from our study of 
singularities of twists that if the three axes are coplanar in addition to 
being parallel, then the twists are linearly dependent. In the planar case, 
this means that if the passive joints are collinear, then the right-hand 
side of equation (3.71) loses rank and the mechanism may not be able to 
move. However, this condition is not sufficient since it may happen that 
£11 is in the range of {£( 2 , £ 21 ) £ 22 } even though they are singular. These 
two different situations are shown in Figure 3.20. Note that switching 
the role of the input and output links changes the singular configurations 
of the mechanism. For example, the singularity shown in Figure 3.20a is 
only a singularity if the left-hand link is chosen as the input link (since 
this choice gives three collinear passive joints). 

The configuration shown in Figure 3.20b is known as an uncertainty 
configuration in the kinematics literature. In this case, it is actually 
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(a) Singular configuration (b) Uncertainty configuration 

Figure 3.20: Potential singular configurations for four-bar mechanisms. 


possible for the mechanism to move instantaneously in two independent 
directions. Examining the structure equations, we see that 


and hence 
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represent two independent, instantaneously admissible velocities. These 
two independent velocities exist only if the mechanism is perfectly aligned 
and hence uncertainty configurations rarely occur in practice. 


5.4 Stewart platform 

Another common example of a parallel mechanism is the Stewart plat¬ 
form, an example of which is shown in Figure 3.21. The mechanism 
consists of two rigid bodies, connected by a set of prismatic joints. Each 
prismatic joint is connected to the rigid body by a spherical joint, allowing 
complete rotational motion. Only the prismatic joints are actuated. 

Stewart platforms are commonly used in aircraft flight simulators to 
move an aircraft cockpit along the motion indicated by the (simulated) 
dynamics of the system. Although the concept of a Stewart platform is 
quite old (it was studied by Stewart in the 1950s [111]), it is only recently 
that the kinematics for a general Stewart platform have been solved in 
complete generality. 

In order to write the structure equation for a Stewart platform, we 
must first take a slight detour and discuss the modeling of a spherical 
joint. For the manipulators we have considered previously, we have al¬ 
ways modeled a spherical joint as three revolute joints intersecting at a 


138 


















Figure 3.21: A Stewart platform with a PUMA robot attached. (Photo 
courtesy of Steve Dubowsky, MIT) 


point. This made physical sense since this is how most actuated spher¬ 
ical wrists are built. However, spherical wrists always have singularities 
when any two of the axes become parallel (it can be shown that this will 
always happen for some choice of joint angles). In a Stewart platform, 
the spherical joints are completely passive and hence will never become 
singular. This requires that we define spherical joints slightly differently 
than spherical wrists. 

The rigid motion generated by a spherical joint has the form 


g(R) 


R (I - R)q 
0 1 


R e 50(3), 


where I? is a free parameter and q is the location of the center of the 
wrist. Similarly, the velocity of a spherical joint has the form 


u s 


— u> x q 
u> 


uj £ R 3 , 
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where w is a free parameter (the velocity). To cast this equation into a 
more useful framework, we rewrite V s as 






y s = 

—e± x q -e 2 x q -e 3 x q 


UJ 2 


ei e 2 e 3 






W 3 _ 


where e* is the ith unit vector in R 3 . Notice that the columns of the 
matrix which defines V s are never linearly dependent. 

We can now write the structure equations for the Stewart platform. 
Let g Si (R Si ) represent the orientation of the ith spherical joint attached 
to the base and represent the ith spherical joint attached to the 

tool. Then, the structure equation for the Stewart platform is given by 


9 st = 9s 1 {Rs 1 )e^ 01 g tl {Rt 1 )9st(O) = ••• = 9 s 6 (Rs 6 )e i6e<> g te (R te )g at { 0 ), 

(3.72) 


where £* and 9i model the motion of the ith prismatic joint. 

Solving the forward kinematics for a Stewart platform is a very diffi¬ 
cult problem due to the large number and complicated form of the con¬ 
straints. Abstractly, given the length of the links, we can solve the struc¬ 
ture equations to find the orientations of the ball and socket joints and 
then determine the configuration of the tool frame. As the problem has 
been specified here, there is an extra degree of freedom in each link cor¬ 
responding to rotation of the prismatic joint about its own axis. This 
further complicates the forward kinematics problem. 

The inverse kinematics problem for the Stewart platform is remark¬ 
ably simple. Given the desired configuration of the platform, we find 
the locations of the pivot points and solve for the distance between each 
base and the appropriate pivot. Let q Si be the location of the ith pivot 
point on the base and q ti be the location of the tool pivot point (written 
relative to the base and tool frames, respectively). Then, the extension 
of the prismatic joints is given by 

= life ~ gstqu li¬ 
lt is possible to derive this result in a manner similar to that used in 
solving the subproblems of Section 3; but in this case, the solution is 
obvious from the geometry of the manipulator. 

We may now study the mobility of the Stewart platform by calcu¬ 
lating the Jacobian of the structure equation. Taking the Jacobian of 
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equation (3.72) yields 


— [f.1,1 £«i,2 £si, 3 Cl Cq, 1 Cti,2 Ct!,3 


— [C«6,l £s 6,2 Cse,3 Cfi Ct 8 ,l Ct 6 ,2 Ct 6 ,3 

It can be shown that the Jacobian matrices are never singular as long as 
9i is nonzero (see Exercise 22). Hence, all tool velocities are admissible. 

However, an interesting problem occurs when the tool frame and base 
frame are coplanar. In this case, the actuated joints can only generate 
forces in the plane, and hence the mechanism cannot resist (or apply) 
nonplanar forces or torques. Note that the mechanism is still not kine¬ 
matically singular: the joints can accommodate any motion of the tool 
frame. However, the actuated joints cannot generate the wrenches nec¬ 
essary to actually achieve any motion. This is an example of the second 
class of singularity that was mentioned previously. We call this type 
of singularity an actuator singularity since it corresponds to a failure of 
the actuated joints to be able to generate arbitrary wrenches in the tool 
frame. This type of singularity is very closely related to the failure of the 
force-closure conditions which occur in grasping. 

A geometric interpretation of this singularity in the Stewart platform 
can be obtained by noting that the system of wrenches which can be 
applied to the tool frame is given by the set of all zero-pitch (pure force) 
wrenches generated by the actuated joints through the points q ti . Since 
the prismatic joints generate zero-pitch wrenches, we can use the previ¬ 
ously derived examples of singularities of zero-pitch screws to locate some 
of the singularities of the Stewart platform. In this context, when the base 
and tool frames are coplanar, we get a singularity because we have four 
(actually six) coplanar, zero-pitch screws. A separate singularity occurs 
whenever two of the prismatic joints are collinear. 

Example 3.18. Singularities for a planar Stewart platform 

Consider the planar parallel mechanism shown in Figure 3.22. Three ac¬ 
tuated prismatic joints are used to control the position and orientation 
of the platform. The revolute joints at the end of each link are pas¬ 
sive. Kinematically, this mechanism shares some of the properties of the 
Stewart platform. 

We concentrate on the singularities of the mechanism. As with the 
Stewart platform, it can be shown that there are no kinematic singu¬ 
larities where the dimension of the space of achievable velocities drops 
rank (it is always three). Since the actuated joints are all prismatic, the 
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Figure 3.22: A planar version of the Stewart platform. 


wrenches generated by the joints correspond to zero-pitch screws. In the 
plane, it can be shown that three zero-pitch screws intersecting at a point 
are singular. This is exactly the configuration in which the mechanism is 
drawn in Figure 3.22 (see the wrench diagram to the right). 

Hence, in this configuration, it is not possible for the mechanism to 
generate pure torques around the point of common intersection. This 
is clear if we write down the wrenches relative to a coordinate frame 
attached at the intersection point. In this set of coordinates, we have 

= |Vl v 2 v 3 

0 0 0 


/i 

h 

fa 


where Vi is the direction of the ith prismatic axis and /* is the force 
exerted by the ith actuator. It is clear that we cannot generate a pure 
torque around the intersection point since 
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6 Summary 


The following are the key concepts covered in this chapter: 

1. The forward kinematics of a manipulator is described by a mapping 
9st • Q SE(3) which describes the end-effector configuration as a 
function of the robot joint variables. For open-chain manipulators 
consisting of revolute and prismatic joints, the kinematics can be 
written using the product of exponentials formula : 

g st (9)=e^e^---e^g st { 0 ), 

where is the twist corresponding to the ith joint axis in the ref¬ 
erence (6 = 0) configuration. 

2. The (complete) workspace of a manipulator is the set of end-effector 
configurations which can be reached by some choice of joint angles. 
The reachable workspace defines end-effector positions which can 
be reached at some orientation. The dextrous workspace defines 
end-effector positions which can be reached at any orientation. 


3. The inverse kinematics of a manipulator describes the relationship 
between the end-effector configuration and the joint angles which 
achieve that configuration. For many manipulators, we can find the 


inverse kinematics 

by making use 

of the following subproblems: 

Subproblem 1: 

II 

<s> 

1) 

rotate one point onto 



another 

Subproblem 2: 

e^ l8l e t 2&2 p = q 

rotate about two 
intersecting twists 

Subproblem 3: 

\\q-e* e p\\ = $ 

move one point to a specified 
distance from another 


To find a complete solution, we apply the manipulator kinemat¬ 
ics to a set of points which reduce the complete problem into an 
appropriate set of subproblcms. 


4. The manipulator Jacobian relates the joint velocities 9 to the end- 
effector velocity V st and the joint torques r to the end-effector 
wrench F: 


v s \ = Jg t (0)9 T = (J s st ) T F s (spatial) 

V s b t = J b st (9)9 t = ( J b st fF t (body). 


If the manipulator kinematics is written using the product of expo¬ 
nentials formula, then the manipulator Jacobians have the form: 


4(0) = [a & ••• &] 
•&(*) = & ••• eU &] 


ft _ Ad -1 £ 

(^•••e«"S st(0) ) 
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5. A configuration is singular if the manipulator Jacobian loses rank 
at that configuration. Examples for a general six degree of freedom 
arm include: 

(a) Two collinear revolute joints 

(b) Three parallel, coplanar revolute joint axes 

(c) Four intersecting revolute joint axes 

The manipulability of a robot provides a measure of the nearness 
to singularity. 

6 . A manipulator is kinematically redundant if it has more than the 
minimally required degrees of freedom. The self-motion manifold 
describes the set of joint values which can be used to achieve a de¬ 
sired configuration of the end-effector. Internal motions correspond 
to motions along the self-motion manifold and satisfy 

Jst{0)9 = 0 . 

7. A parallel manipulator has multiple kinematic chains connecting the 
base to the end-effector. For the case of two chains, the kinematics 
satisfies the structure equation 

9st = e fllSl1 • • • <? st (0) = Z 21 ® 21 • • • e^ e ^g st { 0), 

where is twist for the the jth joint on the ith chain. The Jacobian 
of the structure equation has the form 

V s \ = J^eOQr = J|(0 2 )02, 

where 0* = (6n ,..., 0i ni ). A kinematic singularity occurs when 
the dimension of the space of admissible forces drops rank. Other 
singularities can occur when the set of end-effector forces which can 
be generated by the actuated joints drops rank. 

7 Bibliography 

There is a vast literature on robot kinematics, including a number of text¬ 
books devoted to analysis, design, and control of manipulators. For an 
introductory treatment of the topics presented here, consult the textbook 
by Craig [21]. See also [35, 36, 79, 90, 122]. The product of exponentials 
formula was initially described by Brockett [12]; the presentation given 
here was inspired by the dissertation of Paden [85]. A selection of ad¬ 
vanced topics in the flavor of the tools presented in this section can be 
found in a collection of papers edited by Brockett [13]. 


144 


In terms of bounds on the number of inverse kinematic solutions to a 
six degree of freedom manipulator, Rastegar, Roth and Scheinman [100] 
established a bound of 32 using a non-constructive proof. This bound was 
made constructive by Duffy and Crane [29] and reduced to 16 by Prim¬ 
rose [94]. However, it was Lee and Liang [57] who gave a constructive 
procedure for finding the inverse kinematic solutions for a general ma¬ 
nipulator. The procedure has been refined by Roth and Raghavan [96] 
and Manocha and Canny [64], whose account we follow in this chapter. 
Manseur and Doty [65] gave an example of a robot with 16 inverse kine¬ 
matic solutions. 

The treatment of parallel mechanisms given here is not the standard 
one. For a classical treatment of four-bar and other parallel mechanisms, 
see, for example, Hunt [42]. A detailed description of the four-bar synthe¬ 
sis problem, along with analytical and graphical solution techniques, can 
be found in the book by Erdman and Sandor [30], in addition to other 
textbooks on kinematics and design of mechanisms. 
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(iii) (iv) 


Figure 3.23: Some simple three degree of freedom manipulators. 


8 Exercises 

1. Draw the twists axes for the manipulators shown in Chapter 1. 

2. Show that the forward kinematics map for a manipulator is indepen¬ 
dent of the order in which rotations and translations are performed. 

3. For each of the three degree of freedom manipulators shown in 
Figure 3.23: 

(a) Find the forward kinematics map. 

(b) Solve the inverse kinematics problem using the Paden-Kahan 
subproblems. 

(c) Derive the spatial and body Jacobians. 

4. For each of the manipulators shown schematically in Figure 3.24: 

(a) Find the forward kinematics map. 


146 













































Figure 3.24: Sample manipulators. Revolute joints are represented by 
cylinders; prismatic joints are represented by rectangular boxes. 


(b) Solve the inverse kinematics problem using the Paden-Kahan 
subproblems. 

(c) Derive the spatial and body Jacobians. 

(d) Give a geometric description of the singular configurations. 

(e) Describe the reachable and dextrous workspaces and calculate 
the number of inverse kinematic solutions in different regions 
of the workspace. 

(Note that some of these problems have already been solved in the 
examples.) 

5. Subproblem 2': Rotation about two non-intersecting axes 

Solve Subproblem 2 when the two axes and £2 do not intersect. 
Use this subproblem to solve the inverse kinematics for the elbow 
manipulator in Example 3.5 when the first two joints do not inter¬ 
sect at a point. 

6 . Subproblem 4: Rotation about two axes to given distances 

Let £ 1,62 be two zero-pitch unit magnitude twists with intersecting 
axes, and p. q±, and 52 be points in R 3 (see Figure 3.25). Find 9\ 
and 9 2 such that 

HgU® 1 e ?202 p_ 
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Figure 3.25: Subproblem 4: Rotate p about the axis of £1 followed by a 
rotation about the axis of £2 such that the final location of p is <5i from 
qi and 62 from q 2 - 


and 

|| e «i«i e f202p _ q2 1| _ ,5 2 _ 

(Hint: Find a point q such that q = e^ l 8 l e^ 2 e 2 p, and q is on the 
intersection of the three spheres centered at, respectively, q±, < 72 , 
and r, of radii <5i, 62 , and \\p — r||.) 

7. Subproblem 5: Translation to a given distance 

Let £ be an infinite-pitch unit-magnitude twist; p,q G R 3 two 
points; and S a real number > 0. Find 9 such that 

Ik - ^ e p\\ = S. 

Use this subproblem to solve for the extension of the prismatic joint 
in the SCARA robot in Example 3.6. 

8. Show that the spatial velocity of a manipulator does not depend 
on the location of the tool frame (as long as it moves with the 
end-effector). 

9. Singular values of a matrix 

Let A : R™ —> R p represent a linear map and assume that r is the 
rank of A. Thus, r < min(n,p). Show that there exist matrices 
U G R pxp , V G R nXn and E G R pxn such that 

A = UT,V t 1 (3.74) 
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where 


(a) The columns of V are orthonormal, i.e., V T V = I. Further, it 
may be partitioned as 

V =[vx • • • v r v r+ i ■■■ v n \ = [Vi V 2 \ 

so that the range space of A T : R p —► R", denoted 1Z(A T ), is 
spanned by the columns of Vi, and the null space of A, denoted 
t]{A), is spanned by the columns of V 2 . 

(b) The columns of U are orthonormal, i.e., U T U = /, and it may 
be partitioned as 

U = [ui • • • u r u r+ 1 • • • Up] = [t/i U 2 ] 

so that 71(A) is spanned by the columns of Ui, and r/(A T ) is 
spanned by the columns of U 2 . 

(c) E is a matrix of dimension pxnof the form 


with 


o-i 


Ei = 


0 



0 

0 


0 


G R 


rxr 


(7 


r 


<7l > • • • > <J r > 0 


The a, are called the singular values of A, and E G R pxn is 
the representation of A in terms of the V basis for R 71 and the 
U basis for R p . 


10. Let J(9) : R" —> R 6 be the Jacobian of a manipulator. Show that 
the manipulability measure /Z 3 (9) is given by the product of the 
singular values of J(9); that is, 

6 

M 0 ) = IJ 0i(0)- 

i=1 

Thus, 113 ( 9 ) is zero if and only if the Jacobian is singular. 

11. Let A : 1™ —> R p be of rank r and have singular value decompo¬ 
sition (3.74). Let Bi denote the ball of unit radius in R”; that 
is, 

B 1 = {x G R”, ||*|| < 1}. 

Use the description of the matrices U, V of Exercise 9 to find the 
map under A of B\. Distinguish between the cases that p > n and 
p < n and also when r < min (n,p). 
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12. Let J(9) : K™ —> R p be the Jacobian of a manipulator (jp = 3 or 
6). Assume that a task is modeled by an ellipsoid in the task space 
with its principal axes of length ol\,- ■ ■ ,a p . Let Ep C R p be an 
ellipsoid of size scaled by f3, namely 



Define a manipulability measure on J(9) which takes into account 
the task requirement as 


p t {0) := max{/3 : J(9)(B 1 ) C E 0 }. 


Characterize /x t (0) in terms of the singular values of J{9) and lengths 
of the principal axes, on ,,a p . 

13. Isotropic points 

A point in a manipulator’s workspace is said to be isotropic if the 
condition number of the Jacobian is 1. 

(a) Calculate conditions under which a two-link planar manipula¬ 
tor has isotropic points and sketch their location in the plane. 

(b) Compute the isotropic points for an elbow manipulator with¬ 
out a wrist. 

(c) Discuss why isotropic points are useful for tasks which involve 
applying forces against the environment. 

14. Euler angles can be used to represent rotations via the product of 
exponentials formula. If we think of (a, (3, 7 ) as joints angles of a 
robot manipulator, then we can find the singularities of an Euler 
angle parameterization by calculating the Jacobian of the “forward 
kinematics,” where we are concerned only with the rotation por¬ 
tion of the forward kinematics map. Use this point of view to find 
singularities for the following classes of Euler angles: 

(a) ZYZ Euler angles 

(b) ZYX Euler angles 

(c) XYZ Euler angles 

15. Kinematic singularity: four coplanar revolute joints 

Four revolute joint axes with twists = ( qi x u>i, Wj), i = 1,..., 4, 
are said to be coplanar if there exists a plane with unit normal n 
such that: 

(a) Each axis direction is orthogonal to n: n T u>i = 0, i = 1,..., 4. 
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(b) The vector from c/,; to qj is orthogonal to n: n T (qi — qj) = 0, 
i = 1,..., 4. 

Show that when four of its revolute joint axes are coplanar, a six 
degree of freedom manipulator is at a singular configuration. Give 
an example of a manipulator exhibiting such a singularity. 

16. Kinematic singularity: six revolute joints intersecting along a line 
Six revolute joint axes with twists £* = ( qi x u>i,u>i), i = 1,... ,6, 
intersect along a line (p x n,n) if there exist constants ji, Pi G R, 
i = 1 , ... 6 , such that 

qi + = p + Pin. 

Show that when the six revolute joint axes of a six degree of freedom 
manipulator intersect along a line, the manipulator is at a singular 
configuration. 

17. Kinematic singularity: prismatic joint perpendicular to two parallel 
coplanar revolute joints 

A prismatic joint with twist £3 = ( 1 ) 3 , 0) is normal to a plane con¬ 
taining two parallel revolute axes = ( qi x u>i,Ui), i = 1 , 2 , if 

(a) vjoji = 0 , i = 1,2 

(b) (<?i - q 2 ) = 0 

(c) = ±w 2 

Show that when this occurs, a six degree of freedom manipulator 
is at a singular configuration. Give an example of a manipulator 
exhibiting such a singularity. 

18. In general, the manipulator Jacobian depends on the choice of base 
and tool frames. Determine which of the manipulability measures 
described in Section 4.4 is independent of the choice of base and/or 
tool frames. 

19. Show that if a manipulator is at a singular configuration, then there 
exists an end-effector wrench F which can be balanced without 
applying any joint torques. How is the wrench related to the twists 
which form the columns of the Jacobian? 

20. Consider the slider-crank mechanism shown below: 
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(a) Calculate the number of degrees of freedom of the mechanism. 
Explain why the spatial version of Gruebler’s formula cannot 
be used. 

(b) Calculate the structure equations for the mechanism. 

(c) Calculate the Jacobian of the structure equations; give explicit 
expressions for the instantaneous twists for each of the joints. 

(d) Find the singular configurations of the mechanism if d is the 
active variable. 

(e) Find the singular configurations if 0\ is treated as the active 
variable. Under what conditions (on 1 1 , I 2 , I 3 ) do singular 
configurations exist? 


21. The figure below shows a planar parallel manipulator called a “vari¬ 
able geometry truss.” Three actuated prismatic joints are used to 
control the position and orientation of the platform. The revolute 
joints at the end of each link are passive. Assume that that there 
are no actuator limits. 



(a) Use Gruebler’s formula to calculate the number of degrees of 
freedom of the mechanism. 

(b) Write the structure equations for the mechanism. Be sure to 
clearly define your zero configuration. 

(c) Given g st = ([ 2 , y\, R^), find explicit expressions for d\, <? 2 , 
and d%. 
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(d) Find the spatial Jacobian of the structure equations. Give an 
explicit answer. Use the fact that some links intersect at a 
point to minimize extra calculations. 

(e) Find the singular configurations of the mechanism. In addition 
to kinematic singularities, also identify any actuator singular¬ 
ities. 

22. Stewart platform 

Consider the Stewart platform shown in Figure 3.21. Let 9i repre¬ 
sent the displacement of the ith prismatic actuator. 

(a) Use Gruebler’s formula to compute the number of degrees of 
freedom of the mechanism. 

(b) Show that if 9i > 0 for all i, then the mechanism is not at a 
singular configuration. 

(c) Suppose that we replace the spherical joints in the Stewart 
platform with U-joints (a U-joint consists of two orthogonal 
revolute joints which intersect at a point). Use Gruebler’s 
formula to compute the number of degrees of freedom of the 
mechanism. 

(d) Derive the structure equations for the mechanism in part (c). 
Are there any singular configurations? 
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Chapter 4 

Robot Dynamics and 
Control 


This chapter presents an introduction to the dynamics and control of 
robot manipulators. We derive the equations of motion for a general 
open-chain manipulator and, using the structure present in the dynam¬ 
ics, construct control laws for asymptotic tracking of a desired trajectory. 
In deriving the dynamics, we will make explicit use of twists for repre¬ 
senting the kinematics of the manipulator and explore the role that the 
kinematics play in the equations of motion. We assume some familiarity 
with dynamics and control of physical systems. 


1 Introduction 

The kinematic models of robots that we saw in the last chapter describe 
how the motion of the joints of a robot is related to the motion of the rigid 
bodies that make up the robot. We implicitly assumed that we could 
command arbitrary joint level trajectories and that these trajectories 
would be faithfully executed by the real-world robot. In this chapter, we 
look more closely at how to execute a given joint trajectory on a robot 
manipulator. 

Most robot manipulators are driven by electric, hydraulic, or pneu¬ 
matic actuators, which apply torques (or forces, in the case of linear 
actuators) at the joints of the robot. The dynamics of a robot manipu¬ 
lator describes how the robot moves in response to these actuator forces. 
For simplicity, we will assume that the actuators do not have dynamics 
of their own and, hence, we can command arbitrary torques at the joints 
of the robot. This allows us to study the inherent mechanics of robot 
manipulators without worrying about the details of how the joints are 
actuated on a particular robot. 


155 


We will describe the dynamics of a robot manipulator using a set of 
nonlinear, second-order, ordinary differential equations which depend on 
the kinematic and inertial properties of the robot. Although in principle 
these equations can be derived by summing all of the forces acting on 
the coupled rigid bodies which form the robot, we shall rely instead on 
a Lagrangian derivation of the dynamics. This technique has the advan¬ 
tage of requiring only the kinetic and potential energies of the system to 
be computed, and hence tends to be less prone to error than summing 
together the inertial, Coriolis, centrifugal, actuator, and other forces act¬ 
ing on the robot’s links. It also allows the structural properties of the 
dynamics to be determined and exploited. 

Once the equations of motion for a manipulator are known, the inverse 
problem can be treated: the control of a robot manipulator entails finding 
actuator forces which cause the manipulator to move along a given tra¬ 
jectory. If we have a perfect model of the dynamics of the manipulator, 
we can find the proper joint torques directly from this model. In practice, 
we must design a feedback control law which updates the applied forces 
in response to deviations from the desired trajectory. Care is required in 
designing a feedback control law to insure that the overall system con¬ 
verges to the desired trajectory in the presence of initial condition errors, 
sensor noise, and modeling errors. 

In this chapter, we primarily concentrate on one of the simplest robot 
control problems, that of regulating the position of the robot. There are 
two basic ways that this problem can be solved. The first, referred to as 
joint space control , involves converting a given task into a desired path 
for the joints of the robot. A control law is then used to determine joint 
torques which cause the manipulator to follow the given trajectory. A 
different approach is to transform the dynamics and control problem into 
the task space, so that the control law is written in terms of the end- 
effector position and orientation. We refer to this approach as workspace 
control. 

A much harder control problem is one in which the robot is in contact 
with its environment. In this case, we must regulate not only the position 
of the end-effector but also the forces it applies against the environment. 
We discuss this problem briefly in the last section of this chapter and defer 
a more complete treatment until Chapter 6, after we have introduced the 
tools necessary to study constrained systems. 

2 Lagrange’s Equations 

There are many methods for generating the dynamic equations of a me¬ 
chanical system. All methods generate equivalent sets of equations, but 
different forms of the equations may be better suited for computation 
or analysis. We will use a Lagrangian analysis for our derivation, which 
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relies on the energy properties of mechanical systems to compute the 
equations of motion. The resulting equations can be computed in closed 
form, allowing detailed analysis of the properties of the system. 


2.1 Basic formulation 

Consider a system of n particles which obeys Newton’s second law—the 
time rate of change of a particle’s momentum is equal to the force applied 
to a particle. If we let F) be the applied force on the ith particle, rrii be 
the particle’s mass, and r, be its position, then Newton’s law becomes 

Fi = nriifi r,sR 3 ,i = l,...,n. (4.1) 


Our interest is not in a set of independent particles, but rather in 
particles which are attached to one another and have limited degrees 
of freedom. To describe this interconnection, we introduce constraints 
between the positions of our particles. Each constraint is represented by 
a function gj : R 3 " —> R such that 

9 j(n,---,r n ) = 0 j = l,...,k. (4.2) 


A constraint which can be written in this form, as an algebraic rela¬ 
tionship between the positions of the particles, is called a holonomic con¬ 
straint. More general constraints between rigid bodies—involving rj—can 
also occur, as we shall discover when we study multifingered hands. 

A constraint acts on a system of particles through application of con¬ 
straint forces. The constraint forces are determined in such a way that 
the constraint in equation (4.2) is always satisfied. If we view the con¬ 
straint as a smooth surface in R n , the constraint forces are normal to this 
surface and restrict the velocity of the system to be tangent to the sur¬ 
face at all times. Thus, we can rewrite our system dynamics as a vector 
equation 


F = 


~ mil 0 


" rx " 

i 

o 

3 

3 


. r n _ 


k 

+ F 


i=i 


(4.3) 


where the vectors Ti,..., T/. £ R 3ra are a basis for the forces of constraint 
and Xj is the scale factor for the jth basis element. We do not require that 
Ti,..., Tfe be orthonormal. For constraints of the form in equation (4.2), 
Tj can be taken as the gradient of gj , which is perpendicular to the level 
set gj(r) = 0. 

The scalars Ai,...,A& are called Lagrange multipliers. Formally, we 
determine the Lagrange multipliers by solving the 3 n + k equations in 
equations (4.2) and (4.3) for the 3 n + k variables r £ R 3ra and A £ R fe . 
The A i values only give the relative magnitudes of the constraint forces 
since the vectors Tj are not necessarily orthonormal. 
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This approach to dealing with constraints is intuitively simple but 
computationally complex, since we must keep track of the state of all 
particles in the system even though they are not capable of independent 
motion. A more appealing approach is to describe the motion of the 
system in terms of a smaller set of variables that completely describes the 
configuration of the system. For a system of n particles with k constraints, 
we seek a set of m = 3n — k variables qi,... ,q m and smooth functions 
fi, ■ ■ ■, f n such that 

n = fi(qi,---,q m ) 9 j{r 1 ,...,r n )= 0 

i = l,...,n j = l,...,k. 

We call the qi s a set of generalized coordinates for the system. For a 
robot manipulator consisting of rigid links, these generalized coordinates 
are almost always chosen to be the angles of the joints. The specification 
of these angles uniquely determines the position of all of the particles 
which make up the robot. 

Since the values of the generalized coordinates are sufficient to specify 
the position of the particles, we can rewrite the equations of motion for 
the system in terms of the generalized coordinates. To do so, we also 
express the external forces applied to the system in terms of components 
along the generalized coordinates. We call these forces generalized forces 
to distinguish them from physical forces, which are always represented 
as vectors in R 3 . For a robot manipulator with joint angles acting as 
generalized coordinates, the generalized forces are the torques applied 
about the joint axes. 

To write the equations of motion, we define the Lagrangian , L, as the 
difference between the kinetic and potential energy of the system. Thus, 

L{q, q) = T(q , q) -V(q), 


where T is the kinetic energy and V is the potential energy of the system, 
both written in generalized coordinates. 


Theorem 4.1. Lagrange’s equations 

The equations of motion for a mechanical system with generalized coor¬ 
dinates q G R m and Lagrangian L are given by 


d dL dL ^ 

=Ti * = 

(it UQi OQi 


(4.5) 


where Tj is the external force acting on the ith generalized coordinate. 

The equations in (4.5) are called Lagrange’s equations. We will often 
write them in vector form as 

d^dL _dL_ r 

dt dq dq ’ 
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Figure 4.1: Idealized spherical pendulum. The configuration of the sys¬ 
tem is described by the angles 9 and <j). 

where and T are to be formally regarded as row vectors, though 

we often write them as column vectors for notational convenience. A 
proof of Theorem 4.1 can be found in most books on dynamics of me¬ 
chanical systems (e.g., [99]). 

Lagrange’s equations are an elegant formulation of the dynamics of 
a mechanical system. They reduce the number of equations needed to 
describe the motion of the system from n, the number of particles in the 
system, to m, the number of generalized coordinates. Note that if there 
are no constraints, then we can choose q to be the components of r, giving 
T = an d equation (4.5) then reduces to equation (4.1). In 

fact, rearranging equation (4.5) as 


d dL _dL 
dt dq dq 


= — + T 


is just a restatement of Newton’s law in generalized coordinates: 


— (momentum) = applied force. 


dt 


The motion of the individual particles can be recovered through applica¬ 
tion of equation (4.4). 

Example 4.1. Dynamics of a spherical pendulum 

Consider an idealized spherical pendulum as shown in Figure 4.1. The 
system consists of a point with mass m attached to a spherical joint by a 
massless rod of length l. We parameterize the configuration of the point 
mass by two scalars, 6 and <f>, which measure the angular displacement 
from the z- and x-axes, respectively. We wish to solve for the motion of 
the mass under the influence of gravity. 


159 








We begin by deriving the Lagrangian for the system. The position of 
the mass, relative to the origin at the base of the pendulum, is given by 


r{0,4>) 


l sin 9 cos 4> 
l sin 9 sin <j> 
—I cos 9 


The kinetic energy is 

T = im/ 2 ||f|| 2 = ( 'ti 2 + (1 — cos 2 9)(j > 2 ^j 


(4.6) 


and the potential energy is 

V = —mgl cos 9, 

where g « 9.8 m/sec 2 is the gravitational constant. Thus, the Lagrangian 
is given by 

L(q, q) = - ml 2 (j) 2 + (1 — cos 2 9)<j) 2 'j + mgl cos 9 , 
where q = (9, <j>). 

Substituting L into Lagrange’s equations gives 

= — (ml 2 o\ = ml 2 9 
dt V / 

= ml 2 sin 9 cos 9 (f> 2 ~ mgl sin 9 

= — (ml 2 sin 2 9<pj = ml 2 sin 2 9 (f> + 2ml 2 sin 9 cos 9 9cj) 

= 0 


d dL 
dt d9 
dL 
~89 

d^dL 
dt Qij) 
dL 
dej) 


and the overall dynamics satisfy 


ml 2 

0 


"e 

1 

—ml 2 sin 9 cos 9 cf) 2 

I 

mgl sin 9 

0 

ml 2 sin 2 9 


A 

T* 

2ml 2 sin 9 cos 9 9(j) 

-r 

0 


(4.7) 

Given the initial position and velocity of the point mass, equation (4.7) 
uniquely determines the subsequent motion of the system. The motion 
of the mass in R 3 can be retrieved from equation (4.6). 


2.2 Inertial properties of rigid bodies 

To apply Lagrange’s equations to a robot, we must calculate the kinetic 
and potential energy of the robot links as a function of the joint angles 
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Figure 4.2: Coordinate frames for calculating the kinetic energy of a 
moving rigid body. 


and velocities. This, in turn, requires that we have a model for the mass 
distribution of the links. Since each link is a rigid body, its kinetic and 
potential energy can be defined in terms of its total mass and its moments 
of inertia about the center of mass. 

Let V C R 3 be the volume occupied by a rigid body, and p(r), r £ 
V be the mass distribution of the body. If the object is made from a 
homogeneous material, then p(r) = p, a constant. The mass of the body 
is the volume integral of the mass density: 

m = / p(r) dV. 

Jv 

The center of mass of the body is the weighted average of the density: 

p(r)r dV. 

Consider the rigid object shown in Figure 4.2. We compute the kinetic 
energy as follows: fix the body frame at the mass center of the object 
and let (p, R) be a trajectory of the object relative to an inertial frame, 
where we have dropped all subscripts to simplify notation. Let r £ R 3 be 
the coordinates of a body point relative to the body frame. The velocity 
of the point in the inertial frame is given by 

p + R r 

and the kinetic energy of the object is given by the following volume 
integral: 

T =IJ p(r)\\p + Rr\\ 2 dV. (4.8) 

Expanding the product in the kinetic energy integral yields 

T = \ J v p( r ) (iNI 2 + 2 P T R r + ll^ll 2 ) dV. 
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The first term of the above expression gives the translational kinetic 
energy. The second term vanishes because the body frame is placed at 
the mass center of the object and 


p(r)(p T R)r dV = (p T R) 


p(r)rdV = 0. 


Jv Jv 

The last term can be simplified using properties of rotation and skew- 
symmetric matrices: 


/ p(r){Rr) T (Rr) dV = - / p(r)(RQr) T (Rur) dV 
'v 2 Jy 


= 7^ J^p(,r)(ru) T {ru)dV 
= T (^j p{r)? T rdV S j u) =: 


where u> £ R 3 is the body angular velocity. The symmetric matrix 1 £ 
R 3x3 defined by 



p(r)r 2 dV 


is called the inertia tensor of the object expressed in the body frame. It 
has entries 

Ixx = / p(r)(y 2 + z 2 )dxdydz 
Jv 

Ix V = - / p{r)(xy) dxdy dz, 

Jv 

and the other entries are defined similarly. 

The total kinetic energy of the object can now be written as the sum 
of a translational component and a rotational component, 


T= ^m\\p\\ 2 + 


T Xuj 


= l^ T 


ml 

0 


1 


V b =: -(V b YMV b , 


(4.9) 


where V b = g~ 1 g £ se(3) is the body velocity, and M is called the 
generalized inertia matrix of the object, expressed in the body frame. 
The matrix A4 is symmetric and positive definite. 

Example 4.2. Generalized inertia matrix for a homogeneous bar 

Consider a homogeneous rectangular bar with mass m, length l , width 
w, and height h, as shown in Figure 4.3. The mass density of the bar is 
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Figure 4.3: A homogeneous rectangular bar. 


P = Twh ■ We attach a coordinate frame at the center of mass of the bar, 
with the coordinate axes aligned with the principal axes of the bar. 

The inertia tensor is evaluated using the previous formula: 


1T.rr. 


Lxy 


r m 77 ? f h ^ 2 r w / 2 r 1 ' 2 

U\{y 2 + z2 ) dV = ^\ / / (y 2 + z 2 ) dxdydz 

Jv <‘ w "‘ J-h/2 J-w /2 J-l/2 

= i!k{h {lw,h+lwh ^) = +h2) - 

= -f rr^^'-rr / V2 r ' 2 f ' 2 Mdxdydz 

Jy Iwh Iwh J-h/2 J-w/2 J-l/2 


tv 

m 

Iwh 


ph/2 pw/2 

l-h/2 J-w/2 


(^x 2 y\ l /f /2 ^j dydz = 0. 


The other entries are calculated in the same manner and we have: 


I = 


m 

12 


(w 2 + h 2 ) 
0 
0 


0 


§( l 2 + h 2 ) 

0 


m 

12 


o 

o 

(l 2 + w 2 ) 


The inertia tensor is diagonal by virtue of the fact that we aligned the 
coordinate axes with the principal axes of the box. 

The generalized inertia matrix is given by 


M = 


ml 

0 


0 

1 


m 0 0 0 0 

0 m 0 0 0 

0 0m 0 0 

0 0 0 z%(w 2 +h 2 ) 0 

0 0 0 0 j^{t 2 +h 2 ) 

0 0 0 0 0 


0 

0 

0 

0 

0 

T5 (l 2 +’W 2 ) 


The block diagonal structure of this matrix relies on attaching the body 
coordinate frame at center of mass (see Exercise 3). 
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Figure 4.4: Two-link planar manipulator. 


2.3 Example: Dynamics of a two-link planar robot 


To illustrate how Lagrange’s equations apply to a simple robotic system, 
consider the two-link planar manipulator shown in Figure 4.4. Model 
each link as a homogeneous rectangular bar with mass m, and moment 
of inertia tensor 


li = 


Ixi o o 

0 I V i 0 
0 0 I z i 


relative to a frame attached at the center of mass of the link and aligned 
with the principle axes of the bar. Letting i £ R 3 be the translational 
velocity of the center of mass for the ith link and aq £ R 3 be the angular 
velocity, the kinetic energy of the manipulator is 


T{9,9) = -^"hIKII 2 + + ^m 2 \\v 2 \\ 2 + 


Since the motion of the manipulator is restricted to the xy plane, ||uj|| is 
the magnitude of the xy velocity of the center of mass and uq is a vector 
in the direction of the 0 -axis, with ||wi|| = 9\ and 11 0 ^ 2 11 = @i + 9 2 - 

We solve for the kinetic energy in terms of the generalized coordinates 
by using the kinematics of the mechanism. Let pi = ( Xi , yt , 0) denote the 
position of the ith center of mass. Letting rq and r 2 be the distance from 
the joints to the center of mass for each link, as shown in the figure, we 
have 


xi = rici 
Vi = risi 

X 2 = hci + r 2 ci2 
V 2 = hs 1 +r 2 si 2 


xi = -r 1 s 1 8 1 
Vi = riCiOi 

X2 = ~(hsi + r 2 si2)d 1 - r 2 si 2 0 2 

V2 = (hci + r 2 c l 2)9\ + r 2 ci202, 


where Si = sind^, Sij = sin(di + 9j ), and similarly for Ci and Cij. The 
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kinetic energy becomes 

T(0,0) = -to \(x\ + y\) + -l z i6\ + — TO 2(^2 + 2 / 2 ) + 2 ^ 2 (^ 1 + ^ 2 ) 2 


'oi 

T 

ol + 2/3c 2 S + /3c 2 


(h 


S + /3c 2 S 

h. 


(4.10) 

where 

a = T z i -\-T~2 + rriirf + to 2 (^ + 

/3 = m 2 Zir 2 
<5 = 2*2 + TO 2 r|. 

Finally, we can substitute the Lagrangian L = T into Lagrange’s 
equations to obtain (after some calculation) 


a + 2 j3c2 5 + (3 c2 

"0i 

_L 

— /3s 2 0 2 — (3s2(0i + 9 2 ) 

'0i' 


Tl 

5 + (3c 2 S 

h 


/3s 2 0i 0 

P 2 . 


_ T 2_ 


The first term in this equation represents the inertial forces due to accel¬ 
eration of the joints, the second represents the Coriolis and centrifugal 
forces, and the right-hand side is the applied torques. 

2.4 Newton-Euler equations for a rigid body 

Lagrange’s equations provide a very general method for deriving the equa¬ 
tions of motion for a mechanical system. However, implicit in the deriva¬ 
tion of Lagrange’s equations is the assumption that the configuration 
space of the system can be parameterized by a subset of R ra , where n is 
the number of degrees of freedom of the system. For a rigid body with 
configuration g £ SE( 3), Lagrange’s equations cannot be directly used 
to determine the equations of motion unless we choose a local parame¬ 
terization for the configuration space (for example, using Euler angles to 
parameterize the orientation of the rigid body). Since all parameteriza- 
tions of SE( 3) are singular at some configuration, such a derivation can 
only hold locally. 

In this section, we give a global characterization of the dynamics of a 
rigid body subject to external forces and torques. We begin by reviewing 
the standard derivation of the equations of rigid body motion and then 
examine the dynamics in terms of twists and wrenches. 

Let g = ( p,R ) £ SE( 3) be the configuration of a coordinate frame 
attached to the center of mass of a rigid body, relative to an inertial 
frame. Let / represent a force applied at the center of mass, with the 
coordinates of / specified relative to the inertial frame. The translational 
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equations of motion are given by Newton’s law, which can written in 
terms of the linear momentum mp as 

f = 

Since the mass of the rigid body is constant, the translational motion of 
the center of mass becomes 

/ = mp. (4.12) 

These equations are independent of the angular motion of the rigid body 
because we have used the center of mass to represent the position of the 
body. 

Similarly, the equations describing angular motion can be derived in¬ 
dependently of the linear motion of the system. Consider the rotational 
motion of a rigid body about a point, subject to an externally applied 
torque r. To derive the equations of motion, we equate the change in an¬ 
gular momentum to the applied torque. The angular momentum relative 
to an inertial frame is given by X'u > s , where 

X' = RXR t 

is the instantaneous inertia tensor relative to the inertial frame and u> s is 
the spatial angular velocity. The angular equations of motion become 

r =£<?»•) = ±imRTun, 

where r £ R 3 is specified relative to the inertial frame. Expanding the 
right-hand side of this equation, we have 

r = RXR t lo s + RJR t u s + RXR t lo s 
= IV + RR t 1'uj s + I'RR t u s 
= IV + uj s x IV - IV x w s , 

where the last equation follows by differentiating the identity RR T = I 
and using the definition of lo s . The last term of this equation is zero, and 
hence the dynamics are given by 

IV + w s x IV = r. (4.13) 

Equation (4.13) is called Euler’s equation. 

Equations (4.12) and (4.13) describe the dynamics of a rigid body in 
terms of a force and torque applied at the center of mass of the object. 
However, the coordinates of the force and torque vectors are not written 
relative to a body-fixed frame attached at the center of mass, but rather 
with respect to an inertial frame. Thus the pair (/, r) £ R 6 is not the 
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wrench applied to the rigid body, as defined in Chapter 2, since the 
point of application is not the origin of the inertial coordinate frame. 
Similarly, the velocity pair ( p,u ) s ) does not correspond to the spatial or 
body velocity, since p is not the correct expression for the linear velocity 
term in either body or spatial coordinates. 

In order to express the dynamics in terms of twists and wrenches, we 
rewrite Newton’s equation using the body velocity v b = R T p and body 
force f b = R T /. Expanding the right-hand side of equation (4.12), 

-y-( mp ) = ^~{mR vb ) = Rmv b + Rmv b , 

\JLL XjLU 

and pre-multiplying by R T , the translational dynamics become 

mv b + u b x mv b = f b . (4-14) 

Equation (4.14) is Newton’s law written in body coordinates. 

Similarly, we can write Euler’s equation in terms of the body angular 
velocity u b = R T u s and the body torque r b = R t t. A straightforward 
computation shows that 


XX b + uj b x luj b = r b . (4.15) 

Equation (4.15) is Euler’s equation, written in body coordinates. Note 
that in body coordinates the inertia tensor is constant and hence we use 
1 instead of X' = RXR T . 

Combining equations (4.14) and (4.15) gives the equations of motion 
for a rigid body subject to an external wrench F applied at the center of 
mass and specified with respect to the body coordinate frame: 


'ml 0 


Pi> b l 


u> b x mv b 


0 X 


LU b 

+ 

uj b x Xcu b 

= F b 

L J 







(4.16) 


This equation is called the Newton-Euler equation in body coordinates. 
It gives a global description of the equations of motion for a rigid body 
subject to an external wrench. Note that the linear and angular motions 
are coupled since the linear velocity in body coordinates depends on the 
current orientation. 

It is also possible to write the Newton-Euler equations relative to a 
spatial coordinate frame. This version is explored in Exercises 4 and 5. 
Once again the equations for linear and angular motion are coupled, 
so that the translational motion still depends on the rotational motion. 
In this book we shall always write the Newton-Euler equations in body 
coordinates, as in equation (4.16). 
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3 Dynamics of Open-Chain Manipulators 

We now derive the equations of motion for an open-chain robot manipu¬ 
lator. We shall use the kinematics formulation presented in the previous 
chapter to write the Lagrangian for the robot in terms of the joint angles 
and joint velocities. Using this form of the dynamics, we explore several 
fundamental properties of robot manipulators which are of importance 
when proving the stability of robot control laws. 


3.1 The Lagrangian for an open-chain robot 

To calculate the kinetic energy of an open-chain robot manipulator with 
n joints, we sum the kinetic energy of each link. For this we define a 
coordinate frame, L,;, attached to the center of mass of the ith link. Let 

9sh{0) = e^ lBl •••e^% s { 4 (0) 


represent the configuration of the frame Li relative to the base frame of 
the robot, S. The body velocity of the center of mass of the ith link is 
given by 


4 = .4(0)0, 


where <4 is the body Jacobian corresponding to g s q. has the form 


4 w = [4 ••• 4 o ••• o], 


where 




i-l 




9sh( 0)) 


6 


j < i 


is the jth instantaneous joint twist relative to the ith link frame. To 
streamline notation, we write <4 as Ji for the remainder of this section. 
The kinetic energy of the ith link is 


40 , 0 ) = 2 ( 4 ) 


b ' T M i v b h = -d T j'[{e)M i J i {e)6, 


(4.17) 


where is the generalized inertia matrix for the ith link. Now the total 
kinetic energy can be written as 


1 

T{0,0) = J2 T 4 ^ = : 2^ M (0)^- ( 4 - 18 ) 


The matrix M(9) £ ffi. raxn is the manipulator inertia matrix. In terms of 
the link Jacobians, Ji, the manipulator inertia matrix is defined as 

n 

= (4.19) 

2=1 
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To complete our derivation of the Lagrangian, we must calculate the 
potential energy of the manipulator. Let hi{9) be the height of the center 
of mass of the ith link (height is the component of the position of the 
center of mass opposite the direction of gravity). The potential energy 
for the ith link is 

Vi(6) = mighi(9), 

where rrii is the mass of the ith link and g is the gravitational constant. 
The total potential energy is given by the sum of the contributions from 
each link: 

n n 

V{6) = ^2 v i(e) = Y J m i gh i (e). 

2=1 2=1 

Combining this with the kinetic energy, we have 

n . 

Lie, o) = Y, (W *) ^ W) = 2 e T M{o)b - v(9). 

2=1 

3.2 Equations of motion for an open-chain manipu¬ 
lator 

Let 8 £ R n be the joint angles for an open-chain manipulator. The 
Lagrangian is of the form 

L(8,8) = h T M(8)8-V(8 ), 

where M(9) is the manipulator inertia matrix and V{8) is the potential 
energy due to gravity. It will be convenient to express the kinetic energy 
as a sum, 

n 

L{0,9) = 2 Z M ij{e)8i0j - V(8). (4.20) 

i,j =1 

The equations of motion are given by substituting into Lagrange’s 
equations, 

_ 9L _ 
dtWi d0i~ 

where we let Tj represent the actuator torque and other nonconservative, 
generalized forces acting on the ith joint. Using equation (4.20), we have 

7 p\ Y 7 ^ ^ 

llti) i, = M ^j) = (■ M 

1 j= 1 3= 1 

dL 1 ^ dM kj - • 

dOi 2 ^ dOi k 0 ddi' 
j,k= 1 
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The Mij term can now be expanded in terms of partial derivatives to 
yield 


/ , (0)0j 

j =i 




2 aOi 


dv 


dO. 


i = 1,..., n. 


Rearranging terms, we can write 

n n „ 

E Mij(0)0j + ]T T ijk 0j0 k + — (0) = T, i = 1,..., n, (4.21) 
j =i j,k —i * 

where T t jk is given by 

r « t = 2(^r ^- (422) 

Equation (4.21) is a second-order differential equation in terms of the 
manipulator joint variables. It consists of four pieces: inertial forces, 
which depend on the acceleration of the joints; centrifugal and Coriolis 
forces, which are quadratic in the joint velocities; potential forces, of the 
form ; and external forces, T,;. 

The centrifugal and Coriolis terms arise because of the non-inertial 
frames which are implicit in the use of generalized coordinates. In the 
classical mechanics literature, one identifies terms of the form 0i0j 1 i ^ j 
as Coriolis forces and terms of the form Of as centrifugal forces. The 
functions T{j k are called the Christoff el symbols corresponding to the 
inertia matrix M(0). 

The external forces can be divided into two components. Let r* repre¬ 
sent the applied torque at the joint and define —Nt(0, 0) to be any other 
forces which act on the ith generalized coordinate, including conservative 
forces arising from a potential as well as frictional forces. (The reason 
for the negative sign in the definition of N t will become apparent in a 
moment.) As an example, if the manipulator has viscous friction at the 
joints, then Nj would be defined as 


—Ni(0,0) 


dV 

dOi 


po 


n 


where /3 is the damping coefficient. Other forces acting on the manip¬ 
ulator, such as forces applied at the end-effector, can also be included 
by reflecting them to the joints (via the transpose of the appropriate 
Jacobian). 
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In order to put the equations of motion back into vector form, we 
define the matrix C(0, 9) £ R nX71 as 


Cij(0,e) = Yr ijk 0k 

*:=i 


1 y. / dMij dMik dMhj \ • 

^ w) k 


(4.23) 

We call the matrix C the Coriolis matrix for the manipulator; the vector 
C(9 , 9)8 gives the Coriolis and centrifugal force terms in the equations 
of motion. Note that there are other ways to define the matrix C{9 , 9) 
such that Cij(9,9)9j = Tijk9j9k- However, this particular choice has 
important properties which we shall later exploit. 

Equation (4.21) can now be rewritten as 


M(8)8 + <7(0,0)0 + N(0,9) = t 


(4.24) 


where r is the vector of actuator torques and N{9,9) includes gravity 
terms and other forces which act at the joints. This is a second-order 
vector differential equation for the motion of the manipulator as a func¬ 
tion of the applied joint torques. The matrices M and C, which sum¬ 
marize the inertial properties of the manipulator, have some important 
properties which we shall use in the sequel: 

Lemma 4.2. Structural properties of the robot equations of mo¬ 
tion 

Equation (4.24) satisfies the following properties: 

1. M(9) is symmetric and positive definite. 

2. M — 2 C £ R raxn is a skew-symmetric matrix. 


Proof. Positive definiteness of the inertia matrix follows directly from 
its definition and the fact that the kinetic energy of the manipulator is 
zero only if the system is at rest. To show property 2, we calculate the 
components of the matrix M — 2 C: 


{M-2C) ij = M i:j {9)-2C ij (8) 

n 

= £ 


k =1 


dM Hn dM 'Jp dM ^n . dM kj - 

w ~ k ~ k + -W k 


_ dMkj x dMik x 

— / j o ~r\ vk- 


k =1 


BO* 


d9 1 


Switching i and j shows (M — 2 C) T = — (M — 2 C). Note that the skew- 
symmetry property depends upon the particular definition of C given in 
equation (4.23). □ 
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Property 2 is often referred to as the passivity property since it implies, 
among other things, that in the absence of friction the net energy of the 
robot system is conserved (see Exercise 8). The passivity property is 
important in the proof of many control laws for robot manipulators. 


Example 4.3. Dynamics of a three-link manipulator 

To illustrate the formulation presented above, we calculate the dynamics 
of the three-link manipulator shown in Figure 4.5. The joint twists were 
computed in Chapter 3 (for the elbow manipulator) and are given by 


a = 


"0" 


r 0 1 


r 0 -1 

0 


—lo 


— lo 

0 

0 

£2 = 

0 

-1 

£3 = 

h 

-1 

0 


0 


0 

_ 1. 


L 0 J 


L 0 -1 


To each link we attach a frame Li at the center of mass and aligned with 
principle inertia axes of the link, as shown in the figure: 


9 si i(o) — 


1 

00^ 

^ < 
1_ 


I (ri\ 


I ( h+r 2 ) 

9si 2 (o) — 

\ lo / 

9si 3 ( 0) — 

\ lo / 

u 1 

0 1 

0 1 


With this choice of link frames, the link inertia matrices have the general 
form 


rrii 0 

rrii 

0 rrii 

0 

0 

I X i 0 

Iyi 


0 Izi 


where rrii is the mass of the object and I X i , I y i , and I Z i are the moments 
of inertia about the x- 1 y -, and z -axes of the ith link frame. 
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To compute the manipulator inertia matrix, we first compute the body 
Jacobians corresponding to each link frame. A detailed, but straightfor¬ 
ward, calculation yields 


— Jsl i(0) — 


'0 0 0" 


r ~r\c 2 

0 

O-i 

0 0 0 


0 

0 

0 

0 0 0 

0 0 0 

— ^sZ 2 (0) — 

0 

0 

— r 1 
-1 

0 

0 

0 0 0 

— S2 

0 

0 

. 100 . 


L C2 

0 

0 J 


J 3 = J, 


5 ( 3 ( 0 ) 


— I 2 C 2 —r 2 c 2 3 0 0 

0 Z1S3 0 

0 —r 2 —hc 3 —r 2 

0 -1 -1 

— S 23 0 0 

C 23 0 0 _ 


The inertia matrix for the system is given by 


M(d) 


Mu 

M 12 

M 13 

M 2 i 

m 22 

m 23 

M 3 i 

m 32 

m 33 


Ji M\Ji + J 2 M 2 J 2 + J 3 Ml 3 J 3 . 


The components of M are given by 

Mu = Iyis\ + Iy 3 s\ 3 + Izl + Iz2c\ + I z3 C 23 

+ m 2 r\c\ + m 3 (lic 2 + r 2 c 23 ) 2 

Mi 2 = 0 

Mis = 0 

M 2 i = 0 

M 22 = I x 2 + lx 3 + m 3 Zf + m 2 r\ + m 3 r| + 2m 3 lir 2 c 3 
M 2 3 = I x 3 + m 3 r% + rnshr 2 c 3 

M 3 1 = 0 

Ms 2 = I x 3 + m 3 r| + m 3 lir 2 c 3 
M 33 = I x 3 + m 3 r 2 - 


Note that several of the moments of inertia of the different links do not 
appear in this expression. This is because the limited degrees of freedom 
of the manipulator do not allow arbitrary rotations of each joint around 
each axis. 

The Coriolis and centrifugal forces are computed directly from the 
inertia matrix via the formula 


c ij (e,e) = Y i r ijk e k = 


fc =1 


2 ^ 

k =1 


dMji dM, 


de k 


+ 


ik 


DO, 


dM kj 

dO t 


A very messy calculation shows that the nonzero values of r,.^ are given 
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by: 


Tll2 = (Iy2 — Iz 2 — rri 2 ri)c 2 S 2 + (Iy 3 — Iz3)C23S23 

- m 3 (hc2 + r 2 c 23 )(Zis 2 + r 2 s 2 3 ) 

Tll 3 = ( Iy3 — Iz3)C23S23 ~ m 3 r2S2 3 {llC2 + r 2 C 2 3) 

Ti 2 i = (/y 2 — J 22 — m 2 ri)c 2 s 2 + (/ a 3 — / Z 3 )c 2 3 S 2 3 

- m 3 (hc2 + r 2 c 23 )(Zis 2 + r 2 s 23 ) 

r 131 = (Iy3 — Iz3)C23S23 — rn 3 r2S2 3 (hc2 + r 2 c 2 3 ) 

r 2 ll = ( Iz2 — Iy2 + rn2rf)c2S2 + ( Iz3 ~ Iy3)C23S23 

+ m 3 {hc 2 + r 2 C23){hs2 + r 2 s 23 ) 

r 223 = —lirri3r2S3 
r 232 = —lim3r 2 s 3 
r 233 = —hrri3r2S3 

T311 = {Iz3 — Iy3)C23S23 + Wl 3 r 2 S 2 3 (hc 2 + r 2 C 2 3 ) 
r 3 22 = hm,3r2S3 

Finally, we compute the effect of gravitational forces on the manipu¬ 
lator. These forces are written as 

dV 

where V : R" —> R is the potential energy of the manipulator. For the 
three-link manipulator under consideration here, the potential energy is 
given by 

V{9) = nrnghi(0) + m 2 gh 2 (8) + m 3 gh 3 (d), 

where hi is the the height of the center of mass for the ith link. These 
can be found using the forward kinematics map 

9sh(0) =e^ lf>1 ... e* i8i gsu( 0), 

which gives 

hi(0) = r 0 

h 2 {0) =l 0 -n sin 8 2 

h 3 {8) = lo - h sin 0 2 - r 2 sin(0 2 + 8 3 ). 

Substituting these expressions into the potential energy and taking the 
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derivative gives 


N(0,8) 


dV 


0 

— (rn^gri + m 3 gl \) cosd 2 - m 3 r2 cos( 0 2 + #3)) 
-m 3 gr 2 cos( 0 2 + & 3 )) 


This completes the derivation of the dynamics. ' ' ' 

3.3 Robot dynamics and the product of exponentials 
formula 

The formulas and properties given in the last section hold for any me¬ 
chanical system with Lagrangian L = \0 T M[6)6 — V(0). If the forward 
kinematics are specified using the product of exponentials formula, then 
it is possible to get more explicit formulas for the inertia and Coriolis ma¬ 
trices. In this section we derive these formulas, based on the treatments 
given by Brockett et al. [15] and Park et al. [87]. 

In addition to the tools introduced in Chapters 2 and 3, we will make 
use of one additional operation on twists. Recall, first, that in so(3) 
the cross product between two vectors wi,w 2 £ R 3 yields a third vector, 
oj\Xu> 2 £ R 3 . It can be shown by direct calculation that the cross product 
satisfies 

(uq X UJ 2 )^ = C0\U1 2 — UJ 2 UJ\. 

By direct analogy, we define the Lie bracket on se(3) as 

[£1162] = £16 - £2^1- 

A simple calculation verifies that the right-hand side of this equation has 
the form of a twist, and hence [£i,£ 2 ] £ se(3). 

If £i,£ 2 £ R 6 represent the coordinates for two twists, we define the 
bracket operation [•, ■] : R 6 xR 6 -t R 6 as 

[£1, £2] = (£i£2 - £2£i) ■ ( 4 . 26 ) 

This is a generalization of the cross product on R 3 to vectors in R 6 . 
The following properties of the Lie bracket are also generalizations of 
properties of the cross product: 

= -[£ 2 , 6 ] 

[£i> [£2, £3]] + [£2, [£3, £1]] + [£31 [£1 j £2]] = 0. 

A more detailed (and abstract) description of the Lie bracket operation 
on se(3) is given in Appendix A. For this chapter we shall only need the 
formula given in equation (4.26) 
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We now define some additional notation which we use in the sequel. 
Let Aij £ R 6x6 represent the adjoint transformation given by 

f Ad 7 e «i+i%+i ... e U0i\ i> 3 
Ai = ll i = j (4-27) 

[o i < j. 

Using this notation, the jth column of the body Jacobian for the ith link 
is given by Ad g -i 

Ji{0) = Ad-i [AaCi ••• Auk 0 ••• 0], 

9 sli( 0 ) L 1 

We combine Ad -i with the link inertia matrix by defining the trans- 

9 si i (o) J b 

formed inertia matrix for the ith link: 


M' = Ad T _i Mi Ad -i . (4.28) 

9 sl i (0) 

The matrix M[ represents the inertia of the ith link reflected into the 
base frame of the manipulator. 

Using these definitions, we can obtain formulas for the inertial quan¬ 
tities which appear in the equation of motion. We state the results as a 
proposition. 

Proposition 4.3. Formulas for inertia and Coriolis matrices 

Using the notation defined above, the inertia and Coriolis matrices for 
an open-chain manipulator are given by 

n 

y 7 AfiM'iAij^j 

l=majc(ij) ( 4 _ 29 ) 

1 ( dMij dMik dMkj \ x 

~ddj w) 

where 


= 

Cij(0) = 


fk] T Af k M[Aij^j 

=ma x(i,j) 

+ ^AlM[A lk [A kj ^ k ]). (4.30) 

This proposition shows that all of the dynamic attributes of the ma¬ 
nipulator can be determined directly from the joint twists £», the link 
frames g 31 ,( 0 )■ and the link inertia matrices Mi . The matrices Aij are 
the only expressions in equations (4.29) and (4.30) which depend on the 
current configuration of the manipulator. 


dM i:i 

d0 k 
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Proof. The only term which needs to be calculated in order to prove 
the proposition is For i > j, let g t j £ SE( 3) be the rigid 

transformation given by 

f e~^ i6i ... e"^+ 1 ^+ 1 i > j 
9ij = \r ■ • 

[I i=3, 

so that Aij = Ad gy . Using this notation, if k is an integer such that 
i > k > j, then cjij = gik9kj- We now proceed to calculate for 

i > k > j: 

W k {Alj ^ ] = (' W k (■ 9lj ^ 9 «0) = ijt^ 9 w 1 + 9lj ^ ) 

= Cfc 9kj 9ij + gij ij 9kj £k g lk ''j 

= ^^gik fffcj 9kj + 9kj f.j 9kj 

= £ fc ]. 

For all other values of k, gfis zero. The proposition now follows 
by direct calculation. □ 


Example 4.4. Dynamics of an idealized SCARA manipulator 

Consider the SCARA manipulator shown in Figure 4.6. The joint twists 
are given by 


"O' 


h 


I 1 +I 2 


"0" 

0 


0 


0 


0 

0 

0 

£2 = 

0 

0 

& = 

0 

0 

11 

1 

0 

0 


0 


0 


0 

_ 1 _ 


L 1 J 


L 1 J 


LoJ 


Assuming that the link frames are initially aligned with the base frame 
and are located at the centers of mass of the links, the transformed link 
inertia matrices have the form 


' / o' 


mil 0 


I Pi 


mj mfpi 

-m 1 


0 1 


0 I 


-m£i 1 


where pi is the location of the origin of the ith link frame relative to the 
base frame S. 

Given the joint twists and transformed link inertias A4', the dynam¬ 
ics of the manipulator can be computed using the formulas in Proposi¬ 
tion 4.3. This task is considerably simplified using the software described 
in Appendix B, so we omit a detailed computation and present only the 
final result. The inertia matrix M(Q) £ R 4x4 is given by 


M{0) 


a + /3 + 2y cos 0 2 
(3 + 7 cos 9 2 
6 
0 


(3 + 7 cos 0 2 6 0 

/3 6 0 

6 6 0 

0 0 TO4 
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Figure 4.6: SCARA manipulator in its reference configuration. 


where 

a = I z \+ r\m i + ljm 2 + l\m 3 + l\mi 
(3 = I z 2 + I z 3 + I z 4 + l 2 m 3 + 12 m 4 + rn 2 rl 
7 = hl 2 m .3 + hhm^ + hm 2 r 2 

5 = Iz3 + Cz4- 

The Coriolis matrix is given by 


C(0,9) 


-7 sin 0 2 62 

7 sin 02 0 i 


0 

0 


-7 sin 0 2 ( 0 i + 0 2 ) 0 

0 0 

0 0 

0 0 


0 

0 

0 

0 


The only remaining term in the dynamics is the gravity term, which can 
be determined by inspection since only 04 affects the potential energy of 
the manipulator. Hence, 


N(0,9) 


0 

0 

0 

m 4 .g 


Friction and other nonconservative forces can also be included in N. 
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4 Lyapunov Stability Theory 

In this section we review the tools of Lyapunov stability theory. These 
tools will be used in the next section to analyze the stability properties 
of a robot controller. We present a survey of the results that we shall 
need in the sequel, with no proofs. The interested reader should consult 
a standard text, such as Vidyasagar [118] or Khalil [49], for details. 

4.1 Basic definitions 

Consider a dynamical system which satisfies 

x = f(x,t) x(to) = x o i£l". (4-31) 

We will assume that f(x,t) satisfies the standard conditions for the exis¬ 
tence and uniqueness of solutions. Such conditions are, for instance, that 
/( x, t) is Lipschitz continuous with respect to x, uniformly in t. and piece- 
wise continuous in t. A point x* £ R n is an equilibrium point of (4.31) if 
f(x*,t) = 0. Intuitively and somewhat crudely speaking, we say an equi¬ 
librium point is locally stable if all solutions which start near x* (meaning 
that the initial conditions are in a neighborhood of x*) remain near x* 
for all time. The equilibrium point x* is said to be locally asymptotically 
stable if x* is locally stable and, furthermore, all solutions starting near 
x* tend towards x* as t —> oo. We say somewhat crude because the 
time-varying nature of equation (4.31) introduces all kinds of additional 
subtleties. Nonetheless, it is intuitive that a pendulum has a locally sta¬ 
ble equilibrium point when the pendulum is hanging straight down and 
an unstable equilibrium point when it is pointing straight up. If the pen¬ 
dulum is damped, the stable equilibrium point is locally asymptotically 
stable. 

By shifting the origin of the system, we may assume that the equi¬ 
librium point of interest occurs at x* = 0. If multiple equilibrium points 
exist, we will need to study the stability of each by appropriately shifting 
the origin. 

Definition 4.1. Stability in the sense of Lyapunov 

The equilibrium point x* = 0 of (4.31) is stable (in the sense of Lya¬ 
punov) at t = to if for any e > 0 there exists a 6 (to, e) > 0 such that 

||a;(to)|| < <5 => ||a;(t)|| < e, Vf > t 0 . (4-32) 

Lyapunov stability is a very mild requirement on equilibrium points. 
In particular, it does not require that trajectories starting close to the 
origin tend to the origin asymptotically. Also, stability is defined at a 
time instant to- Uniform stability is a concept which guarantees that the 
equilibrium point is not losing stability. We insist that for a uniformly 
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stable equilibrium point x*, 6 in the Definition 4.1 not be a function of 
to, so that equation (4.32) may hold for all to- Asymptotic stability is 
made precise in the following definition: 

Definition 4.2. Asymptotic stability 

An equilibrium point x* = 0 of (4.31) is asymptotically stable at t = to if 

1. x* = 0 is stable, and 

2. x* = 0 is locally attractive; i.e., there exists 6 (to) such that 

||a;(to)|| < <5 ==> lim x(t) = 0. (4.33) 

t —>oo 

As in the previous definition, asymptotic stability is defined at to- 
Uniform asymptotic stability requires: 

1. x* = 0 is uniformly stable, and 

2. x* = 0 is uniformly locally attractive; i.e., there exists <5 indepen¬ 
dent of to for which equation (4.33) holds. Further, it is required 
that the convergence in equation (4.33) is uniform. 

Finally, we say that an equilibrium point is unstable if it is not stable. 
This is less of a tautology than it sounds and the reader should be sure he 
or she can negate the definition of stability in the sense of Lyapunov to get 
a definition of instability. In robotics, we are almost always interested in 
uniformly asymptotically stable equilibria. If we wish to move the robot 
to a point, we would like to actually converge to that point, not merely 
remain nearby. Figure 4.7 illustrates the difference between stability in 
the sense of Lyapunov and asymptotic stability. 

Definitions 4.1 and 4.2 are local definitions; they describe the behavior 
of a system near an equilibrium point. We say an equilibrium point x* 
is globally stable if it is stable for all initial conditions Xq £ K n . Global 
stability is very desirable, but in many applications it can be difficult 
to achieve. We will concentrate on local stability theorems and indicate 
where it is possible to extend the results to the global case. Notions 
of uniformity are only important for time-varying systems. Thus, for 
time-invariant systems, stability implies uniform stability and asymptotic 
stability implies uniform asymptotic stability. 

It is important to note that the definitions of asymptotic stability do 
not quantify the rate of convergence. There is a strong form of stability 
which demands an exponential rate of convergence: 

Definition 4.3. Exponential stability, rate of convergence 

The equilibrium point x* = 0 is an exponentially stable equilibrium point 
of (4.31) if there exist constants m, a > 0 and e > 0 such that 

\\x(t)\\<me- a ^\\x(to)\\ (4.34) 


180 


4 



-4 x 4 

(a) Stable in the sense of Lyapunov 




(b) Asymptotically stable (c) Unstable (saddle) 

Figure 4.7: Phase portraits for stable and unstable equilibrium points. 


for all ||x(fo)|| < e and t > to- The largest constant a which may be 
utilized in (4.34) is called the rate of convergence. 

Exponential stability is a strong form of stability; in particular, it im¬ 
plies uniform, asymptotic stability. Exponential convergence is important 
in applications because it can be shown to be robust to perturbations and 
is essential for the consideration of more advanced control algorithms, 
such as adaptive ones. A system is globally exponentially stable if the 
bound in equation (4.34) holds for all Xq £ R". Whenever possible, we 
shall strive to prove global, exponential stability. 

4.2 The direct method of Lyapunov 

Lyapunov’s direct method (also called the second method of Lyapunov) 
allows us to determine the stability of a system without explicitly inte¬ 
grating the differential equation (4.31). The method is a generalization 
of the idea that if there is some “measure of energy” in a system, then 
we can study the rate of change of the energy of the system to ascertain 
stability. To make this precise, we need to define exactly what one means 
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by a “measure of energy.” Let B t be a ball of size e around the origin, 
B f = {iel": ||x|| < e}. 

Definition 4.4. Locally positive definite functions (lpdf) 

A continuous function V : R n x R + —> R is a locally positive definite func¬ 
tion if for some e > 0 and some continuous, strictly increasing function 
a : R + -> R, 

V(0,f)=0 and V(x, t) > a(||x||) Vx G B e , Vf > 0. (4.35) 

A locally positive definite function is locally like an energy function. 
Functions which are globally like energy functions are called positive def¬ 
inite functions: 

Definition 4.5. Positive definite functions (pdf) 

A continuous function V : R" x R + —> R is a positive definite function if 
it satisfies the conditions of Definition 4.4 and, additionally, a(p) —> oo 
as p —> oo. 

To bound the energy function from above, we define decrescence as 
follows: 

Definition 4.6. Decrescent functions 

A continuous function V : R" x R + —> R is decrescent if for some e > 0 
and some continuous, strictly increasing function (3 : R + —> R, 

V(x,t) < /3(\\x\\) Vx € B e , Vt > 0 (4.36) 

Using these definitions, the following theorem allows us to deter¬ 
mine stability for a system by studying an appropriate energy function. 
Roughly, this theorem states that when V(x,t) is a locally positive defi¬ 
nite function and V (x, t) < 0 then we can conclude stability of the equi¬ 
librium point. The time derivative of V is taken along the trajectories of 
the system: 

V =™ + ™f. 

x—f(x,t ) dt dx 

In what follows, by V we will mean V\£=f(x,t)- 

Theorem 4.4. Basic theorem of Lyapunov 

Let V(x, t ) be a non-negative function with derivative V along the trajec¬ 
tories of the system. 

1 . If V (x, t) is locally positive definite and V(x,t) < 0 locally in x and 
for all t, then the origin of the system is locally stable (in the sense 
of Lyapunov). 

2. IfV(x,t) is locally positive definite and decrescent, and V(x,t) < 0 
locally in x and for all t, then the origin of the system is uniformly 
locally stable (in the sense of Lyapunov). 
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Table 4.1: Summary of the basic theorem of Lyapunov. 



Conditions on 
V(x,t) 

Conditions on 
~V(x,t) 

Conclusions 

1 

lpdf 

> 0 locally 

Stable 

2 

lpdf, decrescent 

> 0 locally 

Uniformly stable 

3 

lpdf, decrescent 

lpdf 

Uniformly asymptotically 
stable 

4 

pdf, decrescent 

pdf 

Globally uniformly 
asymptotically stable 


3. IfV(x,t ) is locally positive definite and decrescent, and —V{x,t) is 
locally positive definite, then the origin of the system is uniformly 
locally asymptotically stable. 

4 • IfV(x,t) is positive definite and decrescent, and —V(x,t) is pos¬ 
itive definite, then the origin of the system is globally uniformly 
asymptotically stable. 

The conditions in the theorem are summarized in Table 4.1. 

Theorem 4.4 gives sufficient conditions for the stability of the origin 
of a system. It does not, however, give a prescription for determining 
the Lyapunov function V(x,t). Since the theorem only gives sufficient 
conditions, the search for a Lyapunov function establishing stability of 
an equilibrium point could be arduous. However, it is a remarkable fact 
that the converse of Theorem 4.4 also exists: if an equilibrium point is 
stable, then there exists a function V(x,t) satisfying the conditions of 
the theorem. However, the utility of this and other converse theorems is 
limited by the lack of a computable technique for generating Lyapunov 
functions. 

Theorem 4.4 also stops short of giving explicit rates of convergence of 
solutions to the equilibrium. It may be modified to do so in the case of 
exponentially stable equilibria. 

Theorem 4.5. Exponential stability theorem 

x* = 0 is an exponentially stable equilibrium point of x = f{x,t) if and 
only if there exists an e > 0 and a function V ( x , t) which satisfies 

ai\\x\\ 2 < V(x,t) < a 2 \\x\\ 2 
V\i=f(x,t) < -a 3 ||z|| 2 

|||^0,t)|| < a 4 ||x|| 

for some positive constants 01,02,0:3,04, and ||a;|| < e. 
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The rate of convergence for a system satisfying the conditions of The¬ 
orem 4.5 can be determined from the proof of the theorem [102]. It can 
be shown that 


m < 



a > 


«3 

2a 2 


are bounds in equation (4.34). The equilibrium point x* = 0 is globally 
exponentially stable if the bounds in Theorem 4.5 hold for all x. 


4.3 The indirect method of Lyapunov 

The indirect method of Lyapunov uses the linearization of a system to 
determine the local stability of the original system. Consider the system 


* = f{x,t) 


with /(0, t) = 0 for all t > 0. Define 


A(t) 


df(x,t ) 


dx 


x=0 


(4.37) 


(4.38) 


to be the Jacobian matrix of f(x,t ) with respect to x, evaluated at the 
origin. It follows that for each fixed t, the remainder 


fi(x,t) = f(x,t) - A(t)x 


approaches zero as x approaches zero. However, the remainder 
approach zero uniformly. For this to be true, we require the 
condition that 


lim sup 
IMHO 4 >o 


H/iQM)ll 

11*11 


= 0. 


may not 
stronger 

(4.39) 


If equation (4.39) holds, then the system 


i = A(t)z 


(4.40) 


is referred to as the (uniform) linearization of equation (4.31) about the 
origin. When the linearization exists, its stability determines the local 
stability of the original nonlinear equation. 


Theorem 4.6. Stability by linearization 

Consider the system (4.37) and assume 


lim sup 

IMHO t>o 


H/iOM)ll 

11*11 


= 0. 


Further, let A(-) defined in equation (4.38) be bounded. If 0 is a uniformly 
asymptotically stable equilibrium point of (4.40) then it is a locally uni¬ 
formly asymptotically stable equilibrium point of (4.37). 
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Figure 4.8: Damped harmonic oscillator. 


The preceding theorem requires uniform asymptotic stability of the 
linearized system to prove uniform asymptotic stability of the nonlinear 
system. Counterexamples to the theorem exist if the linearized system is 
not uniformly asymptotically stable. 

If the system (4.37) is time-invariant, then the indirect method says 
that if the eigenvalues of 


df(x) 


are in the open left half complex plane, then the origin is asymptotically 
stable. 

This theorem proves that global uniform asymptotic stability of the 
linearization implies local uniform asymptotic stability of the original 
nonlinear system. The estimates provided by the proof of the theorem 
can be used to give a (conservative) bound on the domain of attraction 
of the origin. Systematic techniques for estimating the bounds on the 
regions of attraction of equilibrium points of nonlinear systems is an im¬ 
portant area of research and involves searching for the “best” Lyapunov 
functions. 

4.4 Examples 

We now illustrate the use of the stability theorems given above on a few 
examples. 

Example 4.5. Linear harmonic oscillator 

Consider a damped harmonic oscillator, as shown in Figure 4.8. The 
dynamics of the system are given by the equation 

Mq + Bq + Kq = 0, (4.41) 
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where M, B , and K are all positive quantities. As a state space equation 
we rewrite equation (4.41) as 

dt 

Define x = (q, q) as the state of the system. 

Since this system is a linear system, we can determine stability by 
examining the poles of the system. The Jacobian matrix for the system 
is 

A=[ ° 1 

[-K/M - B/M\ ’ 

which has a characteristic equation 

A 2 + (B/M)X + (K/M) = 0. 


q 


q 

q 


-( K/M)q-(B/M)q 


(4.42) 


The solutions of the characteristic equation are 


-B± sjB 2 - 4 KM 
~ 2 M ’ 

which always have negative real parts, and hence the system is (globally) 
exponentially stable. 

We now try to apply Lyapunov’s direct method to determine expo¬ 
nential stability. The “obvious” Lyapunov function to use in this context 
is the energy of the system, 

V(x,t) = ^Mq 2 + \Kq 2 . (4.43) 


Taking the derivative of V along trajectories of the system (4.41) gives 

V = Mqq + Kqq = - Bq 2 . (4.44) 

The function — W is quadratic but not locally positive definite, since it 
does not depend on q , and hence we cannot conclude exponential sta¬ 
bility. It is still possible to conclude asymptotic stability using Lasalle’s 
invariance principle (described in the next section), but this is obviously 
conservative since we already know that the system is exponentially sta¬ 
ble. 

The reason that Lyapunov’s direct method fails is illustrated in Fig¬ 
ure 4.9a, which shows the flow of the system superimposed with the level 
sets of the Lyapunov function. The level sets of the Lyapunov function 
become tangent to the flow when q = 0, and hence it is not a valid 
Lyapunov function for determining exponential stability. 

To fix this problem, we skew the level sets slightly, so that the flow of 
the system crosses the level surfaces transversely. Define 


V(x,t) = - 


q 

T 

' K 

eM 


q 

q_ 


eM 

M 


q_ 


= \qMq + i qKq + eqMq , 
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(a) (b) 


Figure 4.9: Flow of damped harmonic oscillator. The dashed lines are 
the level sets of the Lyapunov function defined by (a) the total energy 
and (b) a skewed modification of the energy. 


where e is a small positive constant such that V is still positive definite. 
The derivative of the Lyapunov function becomes 

V = qMq + qKq + eMq 2 + eqMq 

= {-B + eM)q 2 + e(-Kq 2 - Bqq) = - 

The function V can be made negative definite for e chosen sufficiently 
small (see Exercise 11) and hence we can conclude exponential stability. 
The level sets of this Lyapunov function are shown in Figure 4.9b. 

This same technique is used in the stability proofs for the robot control 
laws contained in the next section. 


q 

1 

eK |eB 


q 

q 


\eB B-eM 


q 


Example 4.6. Nonlinear spring mass system with damper 

Consider a mechanical system consisting of a unit mass attached to a 
nonlinear spring with a velocity-dependent damper. If x\ stands for the 
position of the mass and X 2 its velocity, then the equations describing the 
system are: 


= x 2 

±2 = ~f{x 2 ) - g(xi). 


(4.45) 


Here / and g are smooth functions modeling the friction in the damper 
and restoring force of the spring, respectively. We will assume that /, g 
are both passive; that is, 


crf{a) > 0 Vo- G [—cr 0 , cr 0 ] 
crg(a) > 0 Vcr e [—cr 0 , a 0 \ 
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and equality is only achieved when a = 0. The candidate for the Lya¬ 
punov function is 

r 2 r Xi 

V (x) = y + J o g(v)dcr. 

The passivity of g guarantees that V ( x ) is a locally positive definite func¬ 
tion. A short calculation verifies that 

V(x) = — *2/(^2) < 0 when \xi\ < cr 0 . 

This establishes the stability, but not the asymptotic stability of the ori¬ 
gin. Actually, the origin is asymptotically stable, but this needs Lasalle’s 
principle, which is discussed in the next section. 

4.5 Lasalle’s invariance principle 

Lasalle’s theorem enables one to conclude asymptotic stability of an equi¬ 
librium point even when —V(x, t) is not locally positive definite. However, 
it applies only to autonomous or periodic systems. We will deal with the 
autonomous case and begin by introducing a few more definitions. We 
denote the solution trajectories of the autonomous system 


x = /(x) (4.46) 

as s(t,xo,to), which is the solution of equation (4.46) at time t starting 
from Xq at to- 

Definition 4.7. u limit set 

The set S C R” is the u> limit set of a trajectory s(-,xo,to) if f° r every 
y £ S, there exists a strictly increasing sequence of times t n such that 

s(t n ,x 0 ,t 0 ) -> y 


as t n —> 00. 

Definition 4.8. Invariant set 

The set M C K n is said to be an (positively) invariant set if for all y £ M 
and to > 0, we have 


s(t,y,t Q ) £ M Vf > t 0 - 

It may be proved that the to limit set of every trajectory is closed and 
invariant. We may now state Lasalle’s principle. 

Theorem 4.7. Lasalle’s principle 

Let V : R n —> R be a locally positive definite function such that on the 
compact set fl c = {x £ R" : V(x) < c} we have V(x) < 0. Define 

S = {x £ D c : V(x) = 0}. 
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As t —> oo, the trajectory tends to the largest invariant set inside S; 
i.e., its u! limit set is contained inside the largest invariant set in S. In 
particular, if S contains no invariant sets other than x = 0, then 0 is 
asymptotically stable. 

A global version of the preceding theorem may also be stated. An 
application of Lasalle’s principle is as follows: 

Example 4.7. Nonlinear spring mass system with damper 

Consider the same example as in equation (4.45), where we saw that with 

t 2 r Xi 

V(®) = y + J g(°)dcr, 

we obtained 

V{x) = -x 2 f(x 2 ). 

Choosing c = min(E(— cr 0 , 0), V(a 0 , 0)) so as to apply Lasalle’s principle, 
we see that 

V(x) < 0 for x G f2 c := {x : V(x) < c}. 

As a consequence of Lasallc’s principle, the trajectory enters the largest 
invariant set in C\{x\,x 2 ■ V = 0} = fi c n{xi, 0}. To obtain the largest 
invariant set in this region, note that 


x 2 (t) = 0 =4- xi(t) = x w ==> x 2 (t) = 0 = -/(0) - g{x w ), 

where X10 is some constant. Consequently, we have that 
g(x 10) = 0 ==> x w = 0 . 

Thus, the largest invariant set inside f2 c D {x\,x 2 : V = 0} is the origin 
and, by Lasalle’s principle, the origin is locally asymptotically stable. 

There is a version of Lasalle’s theorem which holds for periodic sys¬ 
tems as well. However, there are no significant generalizations for non¬ 
periodic systems and this restricts the utility of Lasalle’s principle in 
applications. 


5 Position Control and Trajectory Tracking 

In this section, we consider the position control problem for robot ma¬ 
nipulators: given a desired trajectory, how should the joint torques be 
chosen so that the manipulator follows that trajectory. We would like to 
choose a control strategy which is robust with respect to initial condi¬ 
tion errors, sensor noise, and modeling errors. We ignore the problems of 
actuator dynamics, and assume that we can command arbitrary torques 
which are exerted at the joints. 
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5.1 Problem description 

We are given a description of the dynamics of a robot manipulator in the 
form of the equation 

M(9)9 + C{9,9)9 + N(9,9)=t, (4.47) 

where 0 £ R" is the set of configuration variables for the robot and 
r £ R n denotes the torques applied at the joints. We are also given a 
joint trajectory 9d{-) which we wish to track. For simplicity, we assume 
that 6d is specified for all time and that it is at least twice differentiable. 

If we have a perfect model of the robot and 0(0) = 0^(0), 0(0) = 9 d (0), 
then we may solve our problem by choosing 

r = M(9d)b d + C{9 d , 9 d )6 d + N(9 d , 9 d ). 

Since both 0 and 9d satisfy the same differential equation and have the 
same initial conditions, it follows from the uniqueness of the solutions of 
differential equations that 9{t) = 9 d (t) for all t > 0. This an example of 
an open-loop control law: the current state of the robot is not used in 
choosing the control inputs. 

Unfortunately, this strategy is not very robust. If 0(0) ^ 0^(0), then 
the open-loop control law will never correct for this error. This is clearly 
undesirable, since we almost never know the current position of a robot 
exactly. Furthermore, we have no guarantee that if our starting configu¬ 
ration is near the desired initial configuration that the trajectory of the 
robot will stay near the desired trajectory for all time. For this reason, 
we introduce feedback into our control law. This feedback must be chosen 
such that the actual robot trajectory converges to the desired trajectory. 
In particular, if our trajectory is a single setpoint, the closed-loop system 
should be asymptotically stable about the desired setpoint. 

There are several approaches for designing stable robot control laws. 
Using the structural properties of robot dynamics, we will be able to 
prove stability of these control laws for all robots having those properties. 
Hence, we do not need to design control laws for a specific robot; as 
long as we show that stability of a particular control algorithm requires 
only those properties given in Lemma 4.2 on page 171, then our control 
law will work for general open-chain robot manipulators. Of course, the 
performance of a given control law depends heavily on the particular 
manipulator, and hence the control laws presented here should only be 
used as a starting point for synthesizing a feedback compensator. 

5.2 Computed torque 

Consider the following refinement of the open-loop control law presented 
above: given the current position and velocity of the manipulator, cancel 
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all nonlinearities and apply exactly the torque needed to overcome the 
inertia of the actuator, 

r = M(6)6d + <7(0,0)0 + N{0, 0). 

Substituting this control law into the dynamic equations of the manipu¬ 
lator, we see that 

M(0)0 = M(0)0 d , 

and since M(0) is uniformly positive definite in 9, we have 

0 = 0 d ■ (4.48) 

Hence, if the initial position and velocity of the manipulator matches 
the desired position and velocity, the manipulator will follow the desired 
trajectory. As before, this control law will not correct for any initial 
condition errors which are present. 

The tracking properties of the control law can be improved by adding 
state feedback. The linearity of equation (4.48) suggests the following 
control law: 


r = M{9) (o d - K v e - K p e\ + C{6,6)6 + N(9,0) 


(4.49) 


where e = 6 — Od, and K v and K p are constant gain matrices. When 
substituted into equation (4.47), the error dynamics can be written as: 


M{9) (e + K v e + K p e) = 0. 

Since M(9) is always positive definite, we have 

e + K v e + K p e = 0. (4.50) 

This is a linear differential equation which governs the error between the 
actual and desired trajectories. Equation (4.49) is called the computed 
torque control law. 

The computed torque control law consists of two components. We 
can write equation (4.49) as 

r = M{9)9 d + C9 + N + M(9) {—K v e - K p e). 

v_ v ,_ y 's_ v ✓ 

TTf Tfb 

The term rg is the feedforward component. It provides the amount of 
torque necessary to drive the system along its nominal path. The term 
Tfb is the feedback component. It provides correction torques to reduce 
any errors in the trajectory of the manipulator. 

Since the error equation (4.50) is linear, it is easy to choose K v and 
K p so that the overall system is stable and e —> 0 exponentially ast-^oo. 
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Moreover, we can choose K v and K p such that we get independent expo¬ 
nentially stable systems (by choosing K p and K v diagonal). The following 
proposition gives one set of conditions under which the computed torque 
control law (4.49) results in exponential tracking. 

Proposition 4.8. Stability of the computed torque control law 

If K p ,K v £ R nxn are positive definite, symmetric matrices, then the 
control law (4.49) applied to the system (4.47) results in exponential tra¬ 
jectory tracking. 

Proof. The error dynamics can be written as a first-order linear system: 


d 

e 


0 I ' 


e 

dt 

e 

v 

> 

, o. 


e 


A 


It suffices to show that each of the eigenvalues of A has negative real 
part. Let A £ C be an eigenvalue of A with corresponding eigenvector 
v = ( v±,V 2 ) £ C 2n , v yl 0. Then, 


Vi 


0 

I 


Vi 


V2 

_V2_ 


r K v 

-K v 


yi_ 


K p vi - K v v 2 _ 


It follows that if A = 0 then v = 0, and hence A = 0 is not an eigenvalue 
of A. Further, if A ^ 0, then = 0 implies that iq = 0. Thus, rq, v 2 ^ 0 
and we may assume without loss of generality that ||iq|| = 1. Using this, 
we write 

A 2 = iqA 2 rq = v{\v 2 

= vl(-K p i ,q - K v v 2 ) = -v{K p vi - Xv\K v v\, 

where * denotes complex conjugate transpose. Since a = v\K p vi > 0 
and /3 = v^Kyi q > 0, we have 

A 2 + aX + /3 = 0 a,/3>0 

and hence the real part of A is negative. □ 

The power of the computed torque control law is that it converts a 
nonlinear dynamical system into a linear one, allowing the use of any of a 
number of linear control synthesis tools. This is an example of a more gen¬ 
eral technique known as feedback linearization, where a nonlinear system 
is rendered linear via full-state nonlinear feedback. One disadvantage of 
using feedback linearization is that it can be demanding (in terms of com¬ 
putation time and input magnitudes) to use feedback to globally convert 
a nonlinear system into a single linear system. For robot manipulators, 
unboundedness of the inputs is rarely a problem since the inertia matrix 
of the system is bounded and hence the control torques which must be 
exerted always remain bounded. In addition, experimental results show 
that the computed torque controller has very good performance charac¬ 
teristics and it is becoming increasingly popular. 
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5.3 PD control 


Another approach to controller synthesis for nonlinear systems is to de¬ 
sign a linear controller based on the linearization of the system about an 
operating point. Since the linearization of a system locally determines 
the stability of the full system, this class of controllers is guaranteed to be 
locally stable. In many situations, it is possible to prove global stability 
for a linear controller by explicit construction of a Lyapunov function. 

An example of this design methodology is a proportional plus deriva¬ 
tive (PD) control law for a robot manipulator. In its simplest form, a PD 
control law has the form 


r = -K v e - K p e, (4.51) 

where K v and K p are positive definite matrices and e = 9 — 9 d - Since this 
control law has no feedforward term, it can never achieve exact tracking 
for non-trivial trajectories. A common modification is to add an inte¬ 
gral term to eliminate steady-state errors. This introduces additional 
complications since care must be taken to maintain stability and avoid 
integrator windup. 

Before adding a feedforward term, we first show that the PD controller 
gives asymptotic setpoint stabilization. 

Proposition 4.9. If 9 d = 0 and K v ,K p > 0, the control law (4.51) 
applied to the system (4.47) renders the equilibrium point 9 = 9 d globally 
asymptotically stable. 

Proof. For 9d = 0, the closed-loop system is 

M(9)9 + C(9,9)9 + K v 9 + K p (9 - 9 d ) = 0. (4.52) 

Without loss of generality, we assume that 9 d = 0 (if not, redefine 9' = 
9 — 9 d ). We choose the total energy of the system as our Lyapunov 
function, 

V{9,9)= l -9 T M{9)9+ l -9 T K p 9. 

The function V is (globally) positive definite and decresent. Evaluating 
V along trajectories of (4.52), 

V(0,9) = 9 T M9 + i 9 t M9 + 9 T K P 9 
= —9 t K v 9 + i 9 t (M - 2 C)9, 

and since M — 2 C is skew-symmetric, we have 

V = -9 t K v 9. 


193 


Although K v is positive definite, the function V is only negative semi- 
definite, since V = 0 for 0 = 0 and 0^0. Hence from Lyapunov’s basic 
theorem, we can concluded only stability of the equilibrium point. 

To check for asymptotic stability, we appeal to Lasalle’s principle. 
The set S for which V = 0 is given by 

S'= {(0,0) :0 = O}. 

To find the largest invariant set contained in S , we substitute 0 = 0 into 
the closed loop equations 4.52. This gives 

K p 6 = 0 

(recalling that 0 d = 0) and since K p is positive definite, it follows that the 
largest invariant set contained within S is the single point 0 = 0. Hence, 
the equilibrium point 0 = 0 is asymptotically stable. Q 

Since we are primarily interested in tracking, we consider a modified 
version of the PD control law: 


t = M(9)9 d + <7(0,0)0 d + N{9, 0) - K v e - K p e 


(4.53) 


We call this controller the augmented PD control law. Note that the sec¬ 
ond term in equation (4.53) is different from the Coriolis term (7(0, 0)0. 
The reason for this difference is found in the proof of the following theo¬ 
rem. 


Proposition 4.10. Stability of the PD control law 

The control law (4.53) applied to the system (4.47) results in exponential 
trajectory tracking if K v ,K p > 0. 

Proof. The closed-loop system is 

M(9)e + (7(0,0)e + K v e + K p e = 0. (4.54) 

As in the proof of the previous proposition, using the energy of the system 
as a Lyapunov function does not allow us to conclude exponential stability 
because V is only negative semi-definite. Furthermore, since the system is 
time-varying (due to the 0d(-) terms), we cannot apply Lasalle’s principle. 

To show exponential stability, we adopt the same approach as with 
the spring mass system of the previous section. Namely, we skew the level 
sets of the energy function by choosing the Lyapunov function candidate 

V (e, e, t) = - e T M (0)e + -e T K p e + ee T M (0)e, 
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which is positive definite for e sufficiently small since M(6) > 0 and 
K p > 0. Evaluating V along trajectories of (4.54): 

V = e T Me + ^e T Me + e T K p e + ee T Me + ee T [Me + Me) 

= —e T (K v — eM)e + ^e r (M— 2C)e + ee T (— K p e — K v e — Ce+Me) 

= —e T (K v — eM)e — ee T K p e + ee T (—K v + -M)e 

Choosing e > 0 sufficiently small insures that V is negative definite (see 
Exercise 11) and hence the system is exponentially stable using Theo¬ 
rem 4.5. □ 

If dd = 0, i.e., we wish to stabilize a point, the control law (4.53) sim¬ 
plifies to the original PD control law (4.51). We also note that asymptotic 
tracking requires exact cancellation of friction and gravity forces and re¬ 
lies on accurate models of these quantities as well as the manipulator 
inertia matrix. In practice, errors in modeling will result in errors in 
tracking. 

A further difficulty in using the PD control law is choosing the gains 
K p and K v . The linearization of the system about a given operating 
point 9q gives error dynamics of the form 

M(6*o)e + K v e + K p e = 0. 

Since this is a linear system, it is possible to choose K v and K p to achieve 
a given performance specification using linear control theory. However, 
if we are tracking a trajectory, then there is no guarantee that we will 
remain near and the chosen gains may not be appropriate. In practice, 
one can usually get reasonable results by choosing the gains based on 
the linearization about an equilibrium point in the middle of the robot’s 
workspace. 

5.4 Workspace control 

Suppose we are given a path gd{t) £ SE( 3) which represents the desired 
configuration of the end-effector as a function of time. One way to move 
the manipulator along this path is to solve the inverse kinematics prob¬ 
lem at each instant in time and generate a desired joint angle trajectory 
&d(t) £ Q such that g{0 d {t)) = ga{t). The methods of the previous sec¬ 
tions can then be used to generate a feedback controller which follows 
this path. 

There are several disadvantages to solving the feedback control prob¬ 
lem in this manner. Since solving the inverse kinematics problem is a 
time-consuming task, systems in which gd is specified in real-time must 
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use powerful computers to compute 9d at a rate suitable for control. 
Furthermore, it may be difficult to choose the feedback gains in joint 
space in a meaningful way, since the original task was given in terms of 
the end-effector trajectory. For example, a joint-space, computed torque 
controller with diagonal gain matrices ( K p and K v ) will generate a de¬ 
coupled response in joint space, resulting in straight line trajectories in 
9 if the setpoint of the manipulator is changed. However, due to the 
nonlinear nature of the kinematics, this will not generate a straight line 
trajectory in SE{ 3). For many tasks, this type of behavior is undesirable. 

To overcome these disadvantages, we consider formulating the prob¬ 
lem directly in end-effector coordinates. In doing so, we will eliminate the 
need to solve the inverse kinematics and also generate controllers whose 
gains have a more direct connection with the task performance. However, 
in order to use the tools developed in Section 4, we must choose a set of 
local coordinates for SE( 3), such as parameterizing orientation via Euler 
angles. This limits the usefulness of the technique somewhat, although 
for many practical applications this limitation is of no consequence. This 
approach to writing controllers is referred to as workspace control , since 
x represents the configuration of the end-effector in the workspace of the 
manipulator. 

Let f : Q —> W be a smooth and invertible mapping between the joint 
variables 9 £ Q and the workspace variables In particular, this 

requires that n = p so that the number of degrees of freedom of the robot 
equals the number of workspace variables x. We allow for the possibility 
that p < 6, in which case the workspace variables may only give a partial 
parameterization of SE(3). An example of this situation is the SCARA 
robot, for which the position of the end-effector and its orientation with 
respect to the z-axis form a natural set of coordinates for specifying a 
task. 

The dynamics of the manipulator in joint space has the form 
M{9)9 + C(9,9)9 + N(0,9) = r, 

where r is the vector of joint torques and M, C , and N describe the 
dynamic parameters of the system, as before. 

We can rewrite the dynamics in terms of x £ R p by using the Jacobian 
of the mapping / : 9 i—> x, 

x = J(9)9 m = %- 

Note that J is the Jacobian of the mapping f : Q —> R p and not the 
manipulator Jacobian. Under the assumption that / is smooth and in¬ 
vertible, we can write 

9 = J~ x x and 9 = J~ x x + J~ x )x. 
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We can now substitute these expressions into the manipulator dynamics 
and pre-multiply by J~ T := (J _1 ) T to obtain 

J~ T M(8)J~ 1 x + ^J~ T C(8,9)J~ 1 + J~ T x 

+ J~ t N(9, 9) = J~ t t. 

We can write this in a more familiar form by defining 

M = 

C = J- T (cj~' + M±(J-'j) 

N = J~ t N 
F = J~ t t , 

in which case the dynamics become 

M[9)x + C(8,9)x + N(9,9) = F. (4.55) 

This equation represents the dynamics in terms of the workspace coordi¬ 
nates x and the robot configuration 9. We call M, C, and N the effective 
parameters of the system. They represent the dynamics of the system as 
viewed from the workspace variables. Since / is locally invertible, we can 
in fact eliminate 8 from these equations, and we see that equation (4.55) 
is nothing more than Lagrange’s equations relative to the generalized co¬ 
ordinates x. However, since for most robots we measure 8 directly and 
compute x via the forward kinematics, it is convenient to leave the 9 
dependence explicit. 

Equation (4.55) has the same basic structure as the dynamics for an 
open-chain manipulator written in joint coordinates. In order to exploit 
this structure in our control laws, we must verify that some of the prop¬ 
erties which we used in proving stability of controllers are also satisfied. 
The following lemma verifies that this is indeed the case. 

Lemma 4.11. Structural properties of the workspace dynamics 

Equation (4.55) satisfies the following properties: 

1. M(9) is symmetric and positive definite. 

2. M — 2 C € R raxn is a skew-symmetric matrix. 

Proof. Since J is an invertible matrix, property 1 follows from its defini¬ 
tion. To show property 2, we calculate the M — 2 C: 

M — 2C = J~ t (M - 2C)J~ 1 + j(J _t )MJ _1 - J -t M-^-(J -1 ). 

( JjU hi 
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A direct calculation shows that this matrix is indeed skew-symmetric. □ 

These two properties allow us to immediately extend the control laws 
in the previous section to workspace coordinates. For example, the com¬ 
puted torque control law becomes 

F = M{8) ( x d - K v e - K p e) + C(8,8)x + N{8,8) 
t = J t F, 

where Xd is the desired workspace trajectory and e = x — Xd is the 
workspace error. The proof of stability for this control law is identi¬ 
cal to that given previously. Namely, using the fact that M(0) is positive 
definite, we can write the workspace error dynamics as 

e + K v e + K p e = 0 

which is again a linear differential equation whose stability can be verified 
directly. The PD control law can be similarly extended to workspace 
coordinates. 

The advantage of writing the control law in this fashion is that the 
matrices K v and K p now specify the gains directly in workspace coordi¬ 
nates. This simplifies the task of choosing the gains that are needed to 
accomplish a specific task. Furthermore, it eliminates the need to solve 
for the inverse mapping / -1 in order to control the robot. Instead, we 
only have to calculate the Jacobian matrix for / and its (matrix) inverse. 

Notice that when the manipulator approaches a singular configuration 
relative to the coordinates x, the effective inertia M gets very large. This 
is an indication that it is difficult to move in some directions and hence 
large forces produce very little motion. It is important to note that this 
singularity is strictly a function of our choice of parameterization. Such 
singularities never appear in the joint space of the robot. 

Example 4.8. Comparison of joint space and workspace con¬ 
trollers 

To illustrate some of the differences between implementing a controller 
in joint space versus workspace, we consider the control of a planar two 
degree of freedom robot. We take as our workspace variables the xy 
position of the end-effector. 

Figure 4.10 shows the step response of a computed torque control law 
written in joint coordinates. Note that the trajectory of the end-effector, 
shown on the right, follows a curved path. The time response of the joint 
trajectories is a classical linear response for an underdamped mechanical 
system. 

Figure 4.11 shows the step response of a computed torque control law 
written in workspace coordinates. Now the trajectory of the end-effector, 
including the overshoot, follows a straight line in the workspace and a 
curved line in the joint space. 
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Trajectory in joint coordinates 



Trajectory in Cartesian coordinates 



Joint space trajectory versus time 



Work space trajectory versus time 



Figure 4.10: Step response of a joint space, computed torque controller. 
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Trajectory in Cartesian coordinates 



Work space trajectory versus time 
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Figure 4.11: Step response of a workspace, computed torque controller. 
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6 Control of Constrained Manipulators 


In this short section, we provide a brief treatment of the control of con¬ 
strained manipulators. A more thorough development is given in Chap¬ 
ter 6 . 


6.1 Dynamics of constrained systems 

Consider a problem in which we wish to move the tip of a robot along a 
surface and apply a force against that surface. For simplicity, we assume 
the surface is frictionless, although the analysis presented here can be 
readily extended to the more general case. We suppose that the surface 
we wish to move along can be described by a set of independent, smooth 
constraints 

hj(0 i,...,0„)=O j = l,...,k, (4.56) 

and that there exists a smooth, injective map / : R” -fc —> 1 " such that 

M/i >/^)) = 0. (4-57) 

That is, (f> £ R n-fc parameterizes the allowable motion on the surface and 
9 = f((f>) corresponds to a configuration in which the robot is in contact 
with the surface. 

The control task is to follow a given trajectory (f>d{t) while applying a 
force against the surface. Since the surface is represented in joint space 
as the level set of the map h(9) = 0, the normal vectors to this surface 
are given by the span of the gradients of V/q. (Since the surface is n — k 
dimensional, the dimension of the space of normal vectors is k.) Any 
torques of the form 

T N ='£\ j X7h j (6) (4.58) 

correspond to normal forces applied against the surface. In the absence 
of friction, the work done by these torques is given by 

TN -e = Y, x ^h l -e = Y J ^{^^ 

= E a 4 (/l(0)) = o - 

Hence the normal forces do no work on the system and therefore cause no 
motion in the system. We assume that a desired normal force, specified 
by Ai(£),..., A k(t), is given as part of the task description. 

If the robot remains in contact with the surface, as desired, then the 
dynamics of the manipulator can be written in terms of cj). Differentiating 
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6 = /(</>), w e have 


(4.59) 


n d fl 

e = af* 

•• df ■■ d fdf 
~ d^ + dt 

These equations can be substituted into the robot equations of motion, 
M{6)9 + <7(0,0)0 + N(0, 0) = r 


to yield 

M ^%* + (c(M)|+M OT 4 (g)) * + *<«,*> = t, (4.60) 

where we have left M, C , and iV in terms of 9 to simplify notation. 
Equation (4.60) can be made symmetric by multiplying both sides by 

% . Letting J = ||(</>), we define 

M(0) = J T M(f((j)))J 

CM, </>) = J T (c'(/(^), Ji)J + M{f{4>))j) g 

N{(f>,4>) = J T N{f{4>),J4>) 

F = J t t. 


Using these definitions, the projected equations of motion can be written 
as 

M{<j>)$ + C(<l>,i>)i> + N(<l>,i>)=F. (4.62) 

This equation has the same form as the equation for an unconstrained 
manipulator. We shall show in Chapter 6 that equation (4.62) also sat¬ 
isfies the properties in Lemma 4.2. This is not particularly surprising 
since the coordinates <j> were chosen to be a set of generalized coordinates 
under the assumption that the robot maintains contact with the surface. 

It is important to keep in mind that equation (4.62) represents the 
dynamics of the system only along the surface given by the level sets 
h{9) = 0. By pre-multiplying by J T , we have eliminated the information 
about the forces of constraint. For many applications, we are interested 
in regulating the forces of constraint and hence we must use the full 
equations of motion given in equation (4.60). 

6.2 Control of constrained manipulators 

The control task for a constrained robot system is to simultaneously reg¬ 
ulate the position of the system along the constraint surface and regulate 
the forces of the system applied against this surface. In terms of analyz¬ 
ing stability, it is enough to analyze only the motion along the surface, 
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since no movement occurs perpendicular to the surface. Of course, im¬ 
plicit in this point of view is that we maintain contact with the surface. 
If the manipulator is not physically constrained, this may require that 
we regulate the forces so as to insure that we are always pushing against 
the surface and never pulling away from it. 

In this section we show how to extend the computed torque formalism 
presented earlier to regulate the position and force of the manipulator. 
We give only a sketch of the approach, leaving a more detailed discussion 
until Chapter 6, where we shall see that hybrid position/force control is 
just one example of the more general problem of controlling single and 
multiple robots interacting with each other and their environment. 

We take as given a path on the constraint surface, specified by 
and a normal force to be applied against the surface, specified by the 
Lagrange multipliers Ai , A kit) as in equation (4.58). Since we are 

interested in regulating the force applied against the constraint, it is 
important to insure that the position portion of the controller does not 
push against the constraint. Define 

df ■■ 

t c/> = K v e^, - K p e 0 ) 

where = <f> — (f>d- This is the torque required to move the manipulator 
along the surface while applying no force against the surface. In other 
words, if we apply r = and remove the constraint completely, the 
manipulator will follow the correct path, as if the constraint were present. 

To apply the appropriate normal forces, we simply add tn as defined 
in equation (4.58) to r^. Since rjy is in the normal direction to the 
constraint, it does not affect the position portion of the controller. Of 
course, this requires that the constraint surface actually be present to 
resist the normal forces applied to it. The complete control law is given 

by 

k 

T =T ( f > + E A (4.63) 

i=l 

where is given above. We defer the analysis and proof of convergence 
for this control law until Chapter 6. 

As in the previous control laws, the force control law presented here 
relies on accurate models of the robot and the surface. In particular, we 
note that the applied normal force does not use feedback to correct for 
model error, sensor noise, or other non-ideal situations. 
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Figure 4.12: Planar manipulator moving in a slot. 


6.3 Example: A planar manipulator moving in a slot 

As a simple example of a constrained manipulator, consider the control of 
a two degree of freedom, planar manipulator whose end-effector is forced 
to lie in a slot, as shown in Figure 4.12. This system resembles a slider- 
crank mechanism, except that we are allowed to apply torques on both 
revolute joints, allowing us to control both the motion of the slider as 
well as the force applied against the slot. This example is easily adapted 
to a robot pushing against a wall, in which case the forces against the 
slot must always be pointed in a preferred direction. 

We take the slot to be a straight line passing through the point q = 
( l , 0) and making an angle a with respect to the x-axis of the base frame. 
The vector normal to the direction of the slot is given by 


sm a 


n = 

— cos a 


and the slot can be described as the set of all points pel 2 such that 


{p-q)-n = 0. 

The constraint on the manipulator is obtained by requiring that the 
position of the end-effector remain in the slot. Letting p(9) £ M 2 represent 
the position of the tool frame, this constraint becomes 



Substituting the forward kinematics of the manipulator yields 

h(9) = (h cos + I 2 cos(0i + 02) — 0 sin a 
— (7i sin 0i + l 2 sin(0i + 0 2 )) cos a 
= —l 1 sin(0i — a) — I 2 sin(0i + 0 2 — a) — l sin a. 


203 






















The gradient of the constraint, which gives the direction of the normal 
force, is given by 

v; (a\ — l" - ^ 1 cos(0i - a) -l 2 cos(0 1 + 0 2 - a) 

-h cos(0! + 0 2 - a) 

Note that this is the direction of the normal force in joint coordinates. 
That is, joint torques applied in this direction will cause no motion, only 
forces against the side of the slot. 

To parameterize the allowable motion along the slot, we let s £ R 
represent the position along the slot, with s = 0 denoting the point 
q = (l, 0). Finding a function /(s) such that h(f(s)) = 0 involves solving 
the inverse kinematics of the manipulator: given the position along the 
slot, we must find joint angles which achieve that position. 

If the end of the manipulator is at a position s along the slot, then 
the xy coordinates of the end-effector are 

x(s ) = l + s cos a 
y(s) = ssina. 


Solving the inverse kinematics (see Chapter 3, Section 3) and assuming 
the elbow down solution, we have 


\0i(s)} 


tin -1 ( ssin “ ^ 1 m n-i / s 2 +2Zs cosa+Z 2 +Z 2 -Z 2 ^ 

{l+scosaj 1 COu V 2lWs 2 +2ls cos a+l 2 ) 

K( S )J 


— 1 ( -\-lo — s 2 —21 s cos a—l 2 \ 

^ + cos ( 2 ) 


The Jacobian of the mapping is given by 

— (s+Z cos a)(s +2ls cos a+Z —Z^-Z-Z 2 ) , Z sin ct 

( S 2 +2is'cosa + i 2 + i 2 -i 2 ) ' S 2 +l 2 +2ls COS Of 

4l 2 (s 2 -\-2ls cos a-\-l 2 ) 

2(s-\-l COS a) 
yjAl 2 l 2 — (s 2 +2ls cos a+Z 2 — l 2 — Z|) 2 

(after some simplification). 

This matrix can now be used to compute the equations of motion and 
derive an appropriate control law. In particular, the computed torque 
controller has the form 

t = M{Q)J (. s d - K v e s - K p e s ) + (c{0, 0)J + M(0)j ) s + An, 

where e s = s — s d , A is the desired force against the slot; K v ,K p £ R 
are the gain and damping factors; and M and C are the generalized 
inertial and Coriolis matrices. The inertial parameters were calculated in 
Section 2.3 and are given by 


J = 


2l\(s 2 -\-2ls cos ot+l 2 ) 2 a ; 




M[0) 


a+/3c 2 5+\(3c 2 

5+\(3c2 5 


C(0,0) 


— \fdS 2 O 2 -~\Ps 2 {9l+02) 

\(3s20 1 0 
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where 


a = l z i + 1 Z 2 + mirl + m 2 {ll + rf) 

P = m. 2 lih 
5 = 1z2 + m 2 r2- 

It is perhaps surprising that such a simple problem can have such an 
unwieldy solution. The difficulty is that we have cast the entire problem 
into the joint space of the manipulator, where the constraint 6 = f(s) is 
a very complex looking curve. 

A better way of deriving the equations of motion for this system is to 
rewrite the dynamics of the system in terms of workspace variables which 
describe the position of the end-effector (see Exercise 12). Once written 
in this way, the constraint that the end of the manipulator remain in the 
slot is a very simple one. This is the basic approach used in Chapter 6, 
where we present a general framework which incorporates this example 
and many other constrained manipulation systems. 
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7 Summary 

The following are the key concepts covered in this chapter: 

1. The equations of motion for a mechanical system with Lagrangian 
L = T(q,q) — V(q) satisfies Lagrange’s equations: 

d^dL _ 9L _ 

dtdqi dqt 

where q £ R" is a set of generalized coordinates for the system and 
T £ R n represents the vector of generalized external forces. 

2. The equations of motion for a rigid body with configuration g(t) £ 
SE(3) are given by the Newton-Euler equations: 


ml 

O' 


' v b ' 


tu b x mv b 

0 

1 


L0 b 

+ 

co b x Xuj b 


where m is the mass of the body, X is the inertia tensor, and 
V b = ( v b ,u> b ) and F b represent the instantaneous body velocity 
and applied body wrench. 

3. The equations of motion for an open-chain robot manipulator can 
be written as 

M{6)9 + C(0, 0)0 + N(0, 0)=t 

where 9 £ R” is the set of joint variables for the robot and r £ R n 
is the set of actuator forces applied at the joints. The dynamics of 
a robot manipulator satisfy the following properties: 

(a) M(9) is symmetric and positive definite. 

(b) M — 2 C £ R raxn is a skew-symmetric matrix. 

4. An equilibrium point x* for the system x = /( x, t) is locally asymp¬ 
totically stable if all solutions which start near x* approach x* as 
t —* oo. Stability can be checked using the direct method of Lya¬ 
punov , by finding a locally positive definite function V (a:, t) > 0 
such that —V(x,t) is a locally positive definite function along tra¬ 
jectories of the system. In situations in which —V is only positive 
semi-definite, Lasalle’s invariance principle can be used to check 
asymptotic stability. Alternatively, the indirect method of Lyapunov 
can be employed by examining the linearization of the system, if 
it exists. Global exponential stability of the linearization implies 
local exponential stability of the full nonlinear system. 


206 








5. Using the form and structure of the robot dynamics, several control 
laws can be shown to track arbitrary trajectories. Two of the most 
common are the computed torque control law , 

t = M{6)(0d + K v e + K p e) + C{9,9)9 + 1V(0, 9) : 

and an augmented PD control law , 

t = M(9)9 d + C(9,9)9 d + 1V(0, 9) + K v e + K p e. 

Both of these controllers result in exponential trajectory tracking of 
a given joint space trajectory. Workspace versions of these control 
laws can also be derived, allowing end-effector trajectories to be 
tracked without solving the inverse kinematics problem. Stability 
of these controllers can be verified using Lyapunov stability. 
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exponential stability for PD controllers has been pointed out, for example, 
by Wen and Bayard [120]. 
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9 Exercises 


1. Derive the equations of motion for the systems shown below. 




(a) (b) 

(a) Pendulum on a wire: an idealized planar pendulum whose 
pivot is free to slide along a horizontal wire. Assume that the 
top of the pendulum can move freely on the wire (no friction). 

(b) Double pendulum: two masses connected together by massless 
links and revolute joints. 

2. Compute the inertia tensor for the objects shown below. 



3. Transformation of the generalized inertia matrix 

Show that under a change of body coordinate frame from B to C, 
the generalized inertia matrix for a rigid body is given by 


M c = M T gbc M b M gbc 


ml rnRl c p bc R bc 

-mRlJp bc R bc R bc (l - mpl c )R bc \ ’ 


where g bc denotes the rigid motion taking C to B, and Ai b and 
A4 C are the generalized inertia matrices expressed in frame B and 
frame C. 


4. 


Show that Euler’s equation written in spatial coordinates is given 

by 

l'Co s + w s x T'u 8 = t, 
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where X' = RTR t and r is the torque applied to the center of mass 
of the rigid body, written in spatial coordinates. 

5. Calculate the Newton-Euler equations in spatial coordinates. 

6. Show that it is possible to choose M and C such that the Newton- 
Euler equations can be written as 

MV b + C(g,g)V b = F b , 

where M > 0 and M — 2 C is a skew-symmetric matrix. 

7. Verify that the equations of motion for a planar, two-link manipula¬ 
tor, as given in equation (4.11), satisfy the properties of Lemma 4.2. 

8. Passivity of robot dynamics 

Let H = T + V be the total energy for a rigid robot. Show that if 
M—2C is skew-symmetric, then energy is conserved, i.e., H = 9-r. 

9. Show that the workspace version of the PD control law results in 
exponential trajectory tracking. 

10. Show that the control law 

t = M(9)(9d + Ae) + C{9 , 9)(9d + Ae) -I- AT(0, 9 ) + K v e + Kpt 

results in exponential trajectory tracking when A £ 1 is positive 
and K v ,K p £ K nxn are positive definite [107]. 

11. Show that the matrix 

' eA cB 
eB T C + eD 

is positive definite if A and C are symmetric, positive definite, and 
e > 0 is chosen sufficiently small. 

12. Hybrid control using workspace coordinates 

Consider the constrained manipulation problem described in Sec¬ 
tion 6.3. Let p s t(0) £ be the coordinates of the end-effector 
and let w = p(9 ) represent a set of workspace coordinates for the 
system. 

(a) Compute the matrix J(9) which is used to convert the joint 
space dynamics into workspace dynamics (as in Section 5.4). 

(b) Compute the constraint function in terms of the workspace 
variables and find a parameterization / : R —» R 2 which maps 
the slot position to the workspace coordinates. Let K(s ) rep¬ 
resent the Jacobian of the mapping w = f(s). 
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(c) Write the dynamics of the constrained system in terms of ui and 
its derivatives, the dynamic parameters of the unconstrained 
system, and the matrices J{0) and K{s). 

(d) Verify that the equations of motion derived in step (c) are the 
same as the equations of motion derived in Section 6.3. In 
particular, show that tn and the inertia matrix M(s ) are the 
same in both cases. 
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Chapter 5 

Multifingered Hand 
Kinematics 


In this chapter, we study the kinematics of a multifingered robot hand 
grasping an object. Given a description of the fingers and the object, we 
derive the relationships between finger and object velocities and forces, 
and study conditions under which a grasp can be used to manipulate 
an object. In addition to the usual fixed contact case, we also include 
a complete derivation of the kinematics of grasp when the fingers are 
allowed to roll or slide along the object. 


1 Introduction to Grasping 

Traditional robot manipulators used in industry are composed of a large 
arm with a simple gripper attached as an end-effector. This type of robot 
is effective for tasks in which large motions of the payload are required, 
but it cannot accurately perform precise movements of the payload, such 
as those that might be required in an assembly task. With a traditional 
manipulator, fine motions of a grasped object require precise movements 
of the joints of the robot arm. Due to the size of the links in a typical 
robot manipulator, moving the entire arm is rarely an effective means of 
achieving accurate motions of a grasped object. The situation is analo¬ 
gous to a person trying to write with a pencil by moving his or her entire 
arm. 

A second disadvantage with traditional robot manipulators is that 
for a given gripper, only a small class of objects can be grasped. A 
parallel jaw gripper, for example, is very effective at grasping objects 
which have parallel faces. It cannot, however, be used to “stably” grasp 
a tetrahedron. This limitation is sometimes overcome by equipping the 
robot arm with a tool changer, which allows different grippers to be 
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Figure 5.1: The Utah/MIT hand. (Photo courtesy of Sarcos, Inc.) 


attached to a robot in an efficient fashion. While this effectively extends 
the class of objects which can be lifted, it does not specifically address 
the fine motion problem. 

In this chapter and the next, we investigate the use of multifingered 
robot hands as an alternative method for overcoming these difficulties. 
A multifingered hand is a set of robots which is attached to the end of 
a larger robot arm for the purpose of manipulation. Since a robot hand 
is typically smaller than the robot to which it is attached, it is able to 
improve the overall accuracy of the robot. Further, the extra degrees of 
freedom in a multifingered hand make it possible to grasp a large class of 
objects with a single “end-effector.” 

The price of using a multifingered hand is the complexity of the overall 
system. A robot arm with a multifingered end-effector has many degrees 
of freedom, complicating both the kinematic and dynamic analyses of the 
system. In particular, since the hand is in contact with the object being 
manipulated, we must study the kinematics and dynamics of mechanical 
systems with contact constraints. Additionally, the increased degrees of 
freedom of the system increase the difficulty of planning a feasible grasp 
to perform a given task. 

Because of the added complexity inherent in the use of robot hands, it 
is important to realize that a multifingered robot hand is not the answer 
to every manipulation problem. The use of custom end-effectors can solve 
a large number of problems in such areas as manufacturing and materials 
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transport. Furthermore, custom end-effectors are capable of generating 
stable and rigid grasps by design; a similar grasp using an articulated 
hand requires the use of feedback control and results in an overall de¬ 
crease in the rigidity of the grasp. Nonetheless, for many situations, a 
multifingered robot hand attached to a robot arm is an attractive solu¬ 
tion to a difficult problem. The principles involved in the study of robot 
hands are applicable to a number of other areas, most notably coordi¬ 
nated manipulation between robots. Indeed, if we view two (large) robots 
as fingers of a hand, then the problem of coordinated lifting reduces to 
a problem in grasping. With this point of view in mind, we will present 
the study of multifingered hands in a framework which is applicable to a 
large class of robot systems. 

Overview 

We break the study of multifingered robot hands into two basic parts: 
kinematics and planning, and dynamics and control. In each of these ar¬ 
eas, the techniques presented in previous chapters are extended to account 
for the additional complexity of a robot hand. We will also see many new 
techniques which are unique to the study of multifingered hands, such as 
grasp planning and the kinematics of rolling contact. 

We begin with a detailed study of the kinematics of a multifingered 
robot hand. Given models of an object, a set of robot fingers, and the con¬ 
tact between the fingers and the object, we wish to find the relationship 
between forces and motions of the object and fingers. We will be inter¬ 
ested primarily with the kinematics of this system when the fingers do 
not slip on the object. An implicit assumption in studying the kinematics 
of the hand is that the contact locations are known (or can be measured). 
Under this assumption, we compute the fundamental grasping constraint 
which governs the motion of the hand. 

The grasp planning problem is to determine a set of contact locations 
for the object and the fingers. To do so, we first characterize desirable 
properties of a grasp. These properties include: 

1. The ability to resist external forces. Given a wrench applied to an 
object, we wish to apply contact forces which generate an opposing 
wrench. We will refer to a grasp in which the fingers can resist 
arbitrary external forces as a force-closure grasp. 

2. The ability to dextrously manipulate the object. In order to ma¬ 
nipulate an object, we must be able to move the object in a way 
compatible with the desired task. Depending on the task, this may 
require independent motion in all directions or only some directions. 
We refer to a grasp in which the fingers can accommodate arbitrary 
object motions as a manipulable grasp. 
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Given an object, we seek to choose contacts so that these properties hold 
wherever possible. We will briefly present some procedures for choosing 
the locations of the contacts for some simple cases which illustrate the 
techniques available. 

Chapter 6 studies the dynamics and control problem for multifingered 
robot hands. We extend the dynamic formulation presented in Chapter 4 
to include robotic systems with contact constraints. In fact, it is possible 
to do so in such a way that all of the control laws which can be ap¬ 
plied to robot manipulators can be immediately extended and applied to 
multifingered robot hands. We also present extensions for dynamics and 
control of redundant robots, as these are often present in multifingered 
robot systems. 

Throughout this chapter and the next, we make two assumptions to 
allow precise analysis: 

1. The object is a rigid body in contact with a rigid link robot. 

2. Accurate models of the fingers and object are given. 

The relaxation of these conditions is a topic of current research (see, for 
example, [47, 73]). 


2 Grasp Statics 

We begin by studying a particularly simple case in which all contacts 
between the fingers and the object are idealized as point contacts at a 
fixed location. This case allows one to ignore the possibility that a finger 
rolls or slides along the surface of the object (a possibility which we shall 
study in some detail later). We also begin by ignoring the kinematics of 
the fingers which make up the hand: we consider only the transmission 
of forces between a set of contacts and the object. 

2.1 Contact models 

A contact between a finger and an object can be described as a mapping 
between forces exerted by the finger at the point of contact and the 
resultant wrenches at some reference point on the object. In order to 
simplify the formulation of the dynamics in Chapter 6, we always choose 
the object reference point to be the center of mass of the object. We 
represent the forces at the contacts and on the object in terms of a set 
of coordinate frames attached at each contact location and the object 
reference point. We assume that the location of the contact point on the 
object is fixed. 

For convenience, we shall always choose the contact coordinate frame, 
Ci , such that the 2 -axis of this frame points in the direction of the inward 
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Figure 5.2: Coordinate frames for contact and object forces. 


surface normal at the point of contact (as shown in Figure 5.2). We 
describe the contact location by its relative position and orientation with 
respect to the object reference frame, O. That is, 


9oCi - (PoCi J RoCi ) £ SE(3) 


is the location of the contact with respect to the object. The action of g OCi 
is to take the coordinates of a point given in the contact coordinate frame 
and return the coordinates of the same point in the object coordinate 
frame. The configuration of the object relative to a fixed palm frame is 
given by x Q := g po £ SE( 3). The force applied by a contact is modeled 
as a wrench F Ci applied at the origin of the contact frame, C*. 

Typically, a finger will not be able to exert forces in every direction; 
several simple contact models are used to classify common contact config¬ 
urations. We begin by studying the simplest of these contact types: one 
in which the finger is only allowed to apply normal forces to the object 
at the contact location. We then extend this analysis to include some 
simple models of friction. 

A frictionless point contact is obtained when there is no friction be¬ 
tween the fingertip and the object. In this case, forces can only be applied 
in the direction normal to the surface of the object and hence we can rep¬ 
resent the applied wrench as 



o 

LoJ 


/* > 0 , 


(5.1) 


where f Ci £ R is the magnitude of the force applied by the finger in the 
normal direction. The requirement that f Ci be positive models the fact 
that a contact of this type can push on an object, but it cannot pull on 
the object. 
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Figure 5.3: Frictionless point contact. 


Frictionless point contacts almost never occur in a practical situation, 
but they can serve as a useful model for contacts in which the friction 
between the finger and the object is low or unknown. Since a frictionless 
contact cannot exert forces except in the normal direction, modeling a 
contact as frictionless insures that we do not rely on frictional forces when 
we manipulate the object. 

For grasps in which we do wish to make use of frictional forces, we 
must provide a model for friction. We will use a simple model which 
is often referred to as the Coulomb friction model. We would like to 
describe how much force a contact can apply in the tangent directions to 
a surface as a function of the applied normal force. The Coulomb friction 
model is an empirical model which asserts that the allowed tangential 
force is proportional to the applied normal force, and the constant of 
proportionality is a function of the materials which are in contact. 

If we let /* £ R denote the magnitude of the tangential force and 
f n £ R. denote the magnitude of the normal force, Coulomb’s law states 
that slipping begins when 

where /i > 0 is the (static) coefficient of friction. This implies that the 
range of tangential forces which can be applied at a contact is given by 

I /*|<M/"- (5-2) 

In particular, we see that /" must be positive in order for this relationship 
to hold for at least some non-zero / 4 . 

Equation (5.2) can be represented geometrically, as shown in Fig¬ 
ure 5.4. The set of forces which can be applied at a contact must lie in a 
cone centered about the surface normal. This cone is called the friction 
cone ; the angle of the cone with respect to the normal is given by 

a = tan -1 /i. 
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Figure 5.4: Geometric interpretation of the Coulomb friction model. 

A short table of friction coefficients for common materials is given in 
Table 5.1. Typical values of /i are less than 1, and hence the friction cone 
angle is typically less than 45°. 

A point contact with friction model is used when friction exists be¬ 
tween the fingertip and the object, in which case forces can be exerted in 
any direction that is within the friction cone for the contact. We repre¬ 
sent the wrench applied to the object with respect to a basis of directions 
which are consistent with the friction model: 


10 0 
0 10 



(5.3) 


0 0 0 
Lo o oJ 


where 


\]fl + /! < m/3, h > 0}. 


FC Ci = {/ g R 3 : 


A more realistic contact model is the soft-finger contact. Here we 
allow not only forces to be applied in a cone about the surface normal, 
but also torques about that normal. For simplicity, we model the torques 
as being limited by a torsional friction coefficient. The applied contact 
wrench is 


f _ o o l o r f G pn 

r Ci — 0 0 0 0 Jd JCi c r ^Ci 


10 0 0 
0 10 0 
0 0 10 


(5.4) 


0 0 0 0 
Lo o o i J 


and the friction cone becomes 


FC Ci ={f el 4 : yj f? + /| < p/ 3 , f 3 > 0, |/ 4 | < 7 / 3 } 


where 7 > 0 is the coefficient of torsional friction. 
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Table 5.1: Static friction coefficients for some common materials. 
(Source: CRC Handbook of Chemistry and Physics) 


Steel on steel 

0.58 

Wood on wood 

0.25-0.5 

Polyethylene on steel 

0.3-0.35 

Wood on metals 

0.2-0.6 

Polyethylene on self 

0.5 

Wood on leather 

0.3-0.4 

Rubber on solids 

1-4 

Leather on metal 

0.6 


In general, we model a contact using a wrench basis, B Ci £ R pxmi , 
and a friction cone, FC Ci . In all of our examples, we chose p = 6, the 
dimension of the space of generalized forces that can be applied in SE( 3). 
Other choices are possible, the most common being p = 3, which is used 
for planar grasping. The dimension of the wrench basis, mi, indicates the 
number of independent forces that can be applied by the contact. We 
require that FC Ci satisfy the following properties: 

1. FC Ci is a closed subset of R mi with non-empty interior. 

2. /i,/j€ FC Ci =► ah + 13f 2 G FC Ci for a, (3 > 0. 

The set of allowable contact forces applied by a given contact is: 

F Ci = B c .f c . f Ci £ FC Ci . (5.5) 

Several common contact types are summarized in Table 5.2. Other con¬ 
tacts, such as line and face contacts, are explored in the exercises. 


2.2 The grasp map 

To determine the effect of the contact forces on the object, we transform 
the forces to the object coordinate frame. Let (p OCi ,I? OCi ) be the config¬ 
uration of the ith contact frame relative to the object frame. Then, the 
force exerted by a single contact can be written in object coordinates as 


F 0 = Ad'-i F Ci = 

iJocj 


Rn 


0 


Poe, Roc, Ro 


Be, fa, fei G FC c , 


The matrix Acl T _i is the wrench transformation matrix which maps 

9oCi ^ 

contact wrenches to object wrenches. For brevity we shall often write 
(p Ci ,R Ci ) f° r the configuration of the ith contact frame, dropping the o 
subscript. We define the contact map, Gi G K. pxrni , to be the linear map 
between contact forces, represented with respect to B Ci , and the object 
wrench: 

Gi := Ad^-i B c . 

y° c i 
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Table 5.2: Common contact types. 


Contact type 


Frictionless 
point contact 


Picture 



Wrench basis 


0 

0 

1 

0 

0 

0 


FC 


A >0 



Point contact 
with friction 


1 0 0 
0 10 
0 0 1 
0 0 0 
0 0 0 
0 0 0 


V fl + /I < M/s 

/a > 0 



Soft-finger 


10 0 0 
0 10 0 
0 0 10 
0 0 0 0 
0 0 0 0 
0 0 0 1 


'SJT+JZ < m/s 
h > 0 
I/4I < 7/3 


If we have k fingers contacting an object, the total wrench on the 
object is the sum of the object wrenches due to each finger. The map 
between the contact forces and the total object force is called the grasp 
map , G : —> R p , m = m\ + ■ ■ ■ + m Since each contact map is linear 

and wrenches can be superposed (as long as they are all written in the 
same coordinate frame), the net object wrench is 


F 0 — Gi/ci + • • • + Gkf Ck 


[Gi 


Gfc] 


7d' 

Jch. 


and the grasp map is 




Ad b k 


With this definition, the object wrench can be written 


F 0 = Gf c f c G FC, 


(5.6) 


(5.7) 
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where 


fc = (/ Cl , ■ ■ • ,/cJ e M m 

FC = FC Cl x • • • x FC Ck C R m 

m = mi + ■ ■ ■ + mk- 

Thus, a grasp is completely described by the grasp map G and the friction 
cone FC. 

Definition 5.1. Representation of a grasp 

A complete description of a grasp consists of a matrix G £ R pxm and a 
set FC C R m which satisfies the following properties: 

1. FC is a closed subset of R m with non-empty interior. 

2. fi,f 2 £ FC => ah + Pf 2 £ FC for a,/? > 0. 

The set of wrenches that can be applied to the object by the contacts has 
the form 

Fa = Gf c f c £ FC. 

From now on, we will represent a grasp by the quantities G and FC. 
We often omit explicit mention of the friction cone and refer to G as a 
grasp. 

Example 5.1. Grasp map for frictionless point contacts 

Consider first the case of several point contacts touching an object. Then, 
each contact wrench can be written as 


F 0 




"0" 

R Ci 

0 


0 

1 

Pci R Ci 

Ra 


0 

0 



Lo J 


Performing the matrix multiplication, we see that the object wrench has 
the form 


F n = 


n Ci 

Pa x n Ci 


where n Ci is the direction of the inward surface normal written in the 
object coordinate frame. Combining the effect of each of the fingers 
yields: 


F n = 


,L Ci 

Pa x n Cl 


Pa 


Ck 

x n r . 


'fa 

Jc k _ 


Gf c , 


F a £ R 6 
fa > 0. 


Example 5.2. Soft-finger grasp of a box 

Consider a box grasped by two soft-finger contacts, as shown in Fig¬ 
ure 5.5. The position of the contact frame with respect to the object 
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frame is given by 



0 

1 

0 " 


' 0 ■ 


'1 

0 

0 ' 


'o' 

-R Cl = 

0 

0 

1 

II 

—r 

Rc 2 = 

0 

0 

-1 

Pc 2 = 

r 


1 

0 

0 


0 


0 

1 

0 


0 


The grasp map for each finger is obtained by transforming the standard 
wrench basis into the object coordinate frame: 


Gi = 


R Ci 
Pci R Ci 


o 

R Ci 


'1 0 0 0 ' 
0 10 0 
0 0 10 
0 0 0 0 
0 0 0 0 
Lo o o i J 


Calculating and combining these gives the grasp map, 


0 

1 

0 

0 

1 

0 

0 

0 

0 

0 

1 

0 

0 

0 

-1 

0 

1 

0 

0 

0 

0 

1 

0 

0 

—r 

0 

0 

0 

0 

+r 

0 

0 

0 

0 

0 

1 

0 

0 

0 

-1 

0 

+r 

0 

0 

—r 

0 

0 

0 


with contact forces 


f = (f 1 f 2 f 3 f 4 f 1 f 2 f 3 f 4 1 f 

J C \ J ci 5 J Cl 5 J C± 1 J Cl 5 J C2"> J C2"> J C2"> J C2 ) ^ 


The friction cone in this coordinate frame is given by: 


FC = FC C1 x FC C2 

FC C1 = {f c : + (/ 2 J 2 < pfl, |/ C 4 J < 7/ci) fl > 0} 

= {/c : ^(/c 1 ,) 2 + ( fl ) 2 < m/ c 3 2 , I/ c 4 2 I < 7/ c 3 2 , / 3 > o} • 
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Figure 5.6: Planar grasping. 


Example 5.3. Planar grasp of a rectangle 

Consider the planar grasp shown in Figure 5.6. Rather than analyze this 
grasp in SE( 3), we specialize our results to SE( 2). A wrench in SE( 2) 
is represented by a linear component / G R 2 and an angular component 
r G R. / corresponds to the forces in the plane and r to the torque 
about the normal to the plane (see Exercise 2.11). Wrenches in SE( 2) 
are transformed by the rule 


'fo 


Ra 0 

7c' 

Jo 


\~Py R Ci 1 

T a. 


where R Ci G SO( 2) and p Ci = ( p x ,p y ) G R 2 represents the location of Cj 
relative to the object reference frame. 

Given these definitions, we can proceed to analyze Figure 5.6. In 
SE( 2), we shall choose contact coordinate frames such that the y -axis 
points in the direction of the inward surface normal. With this choice, 
the contact locations are 


R Cl = 

' 0 

1 

Rc 2 = 

'0 

-1' 

-1 

0 

1 

0 


—r 



r 


II 

0 


Pc 2 = 

0 



A wrench basis for a point contact with friction is 


B 


Ci 


1 0 
0 1 
0 0 


with the friction cone constraint written as 


U e FC Ci 

FC Ci = {/ G R 2 : I/ll < m/2, h > 0}. 
Finally, the grasp map is given by 





' 0 

1 

0 

-1' 

G = 

Ad" B Cl Ad—-i B C2 

i/oc^ iJoC2 

= 

-1 

0 

1 

0 




r 0 

r 

0 
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3 Force-Closure 


An important property of a grasp is the ability to balance external object 
wrenches by applying appropriate finger wrenches at the contact points. 
For example, if we are using a multifingered hand to lift an object from 
a table, we must be able to exert forces on the object which act in the 
opposite direction to gravity. Depending on the task, we may also need 
to resist wrenches in other directions. Doing so is complicated by the fact 
that we must insure that the applied finger forces remain in the friction 
cone at all times so as to avoid slippage of the fingers on the surface of 
the object. 


3.1 Formal definition 

If a grasp can resist any applied wrench, we say that such a grasp is 
force-closure. 1 Formally, we make the following definition: 

Definition 5.2. Force-closure grasp 

A grasp is a force-closure grasp if given any external wrench F e £ 
applied to the object, there exist contact forces f c £ FC such that 

Gf c = -F e . 


The following proposition follows directly from the definition. 

Proposition 5.1. Characterization of all force-closure grasps 

A grasp is force-closure if and only if G(FC) = R p . 

A key feature of a force-closure grasp is the existence of internal forces. 
An internal force is a set of contact forces which result in no net force on 
the object. In the previous (planar) example, we see that 


fw = 


0 

1 

0 

1 


Gf N = 0. 


This motivates the following definition. Let int(FC) denote the interior 
of the friction cone. 


Definition 5.3. Internal forces 

If /at £ A f(G) D FC, then /jv is an internal force. If /jv £ A f(G) and 
fi v £ int(-FC), then it is called a strictly internal force. 

1 To be consistent with the literature, we use the term force-closure instead of 
wrench-closure. 
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Internal forces can be used to insure that contact forces satisfy friction 
cone constraints. Note that an internal force is a set of contact forces , 
represented with respect to the wrench basis at each contact. The fol¬ 
lowing proposition shows that the existence of a strictly internal force is 
a necessary condition for a grasp to be force-closure. 

Proposition 5.2. Necessity of internal forces 

A grasp is force-closure if and only if G is surjective and there exists a 
vector of contact forces /jv £ A f(G) such that fw £ int (FC). 

Proof. (Sufficiency) Choose F 0 £ R p and let /' be any vector such that 
F (> = Gf' c . Since G is surjective, such an f[. must exist. We will show 
that there exists an a such that /' + a/jv £ int (PC'). Notice that 

lim ^ c a ^ N = f N £ int (PC'); 

Oi —► OO Q 

therefore, there exists a' sufficiently large such that 

^ + a ' fN £ int (PC) C PC. 
oc 

From the properties of the friction cone, it follows that 
fc ■= fc + a'/jv e int (PC) 


and G/ c = Gf' c = F 0 . 

(Necessity) Suppose that a grasp is force-closure. Choose fi £ int(PC) 
such that F 0 = Gf\ ^ 0. Choose fi £ PC such that G /2 = — F a 
and define / n = fi + fi- Then, we see that Gf n = F 0 — F 0 = 0 and 
/jv G int(PC) from the properties of a cone. □ 

3.2 Constructive force-closure conditions 

Verifying that a grasp is force-closure can be difficult due to the form of 
the friction cone. In certain simple cases, however, it is possible to verify 
that a grasp is force-closure directly from the definition. For example, if 
a grasp consists only of frictionless point contacts, then we saw that the 
grasp map had the form 

n Ck 

Pc k x n Ck _ 

fi > o}. 


n Cl 

Pa X n Cl ■■■ 

FC = {/ £ R k : 


Thus, G(PC) = R 6 if and only if positive linear combinations of the 
columns of G span R 6 . 
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separating 

hyperplane 


(a) 


(b) 


Figure 5.7: Convex hull and separating hyperplane. 


This example shows that the force-closure problem can be reformu¬ 
lated as a problem in positive linear spaces. This motivates the following 
set of definitions; see [97] for a more detailed introduction. 

A set of vectors {t>i, • • • ,Vk} with Vi £ R p is positively dependent if 
there exist a.i > 0 such that 


k 



A set of vectors {i>i, • • • , Vk} positively spans R” if for every x £ R” there 
exists oti > 0, i = 1, • • • ,k such that 


k 



Any set of vectors which positively spans R” is positively dependent 
(by choosing x = 0). Note that positively dependent vectors must have 
strictly positive coefficients: a, > 0. 

A set K is said to be convex if for every x, y £ K, Xx + (1 — A )y £ K , 
A £ [0, f]. Thus any line connecting two points in a convex set lies in that 
set. The convex hull of a set S, denoted co(S'), is the smallest convex set 
K containing S (i.e., it is the intersection of all convex sets containing S). 
Using these definitions, it can be shown that given a set S = {ui, • • • , Ufc}, 
the convex hull of S is 


co(S') = {v = a i v £ Y2 a i = 1> a i > 0}- 


This concept is illustrated in Figure 5.7a. 

A useful object in the study of convex sets is the separating hyper¬ 
plane. Given a point c £ R p , we define a hyperplane through c with 
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normal v £ R p as 


H v {c) = {i £ P : v T {x — c) = 0}. 


It is a basic theorem in convex analysis that given any convex set K and 
a point c ^ K, there exists a hyperplane H v (c) such that for all x £ K, 
v T (x — c) > 0. That is, all points in K lie on the “same side” of H v {c). 
Such a hyperplane is called a separating liyperplane between K and c 
(see Figure 5.7b). If c lies on the boundary of K : then we replace the 
condition v T (x — c) > 0 by v T (x — c) > 0 and we say H v {c) is a supporting 
hyperplane. 

The following proposition shows the utility of these concepts in de¬ 
termining if a grasp is force-closure. 

Proposition 5.3. Convexity conditions for force-closure grasps 

Consider a fixed contact grasp which contains only frictionless point con¬ 
tacts. Let G £ R pxm be the associated grasp matrix and let {Gi} denote 
the columns ofG. The following statements are equivalent: 

1. The grasp is force-closure. 

2. The columns of G positively span R p . 

3. The convex hull of {Gi} contains a neighborhood of the origin. 

f. There does not exist a vector v £ R p , » / 0, such that for i = 
1 ,... ,m, v ■ Gi > 0. 

The equivalence of conditions (1) and (2) follows from the definition of 
force-closure for a grasp with frictionless point contacts. The remaining 
conditions rely on tools from convexity theory which are beyond the scope 
of this book. See [97] for details. 

The fourth condition of Proposition 5.3 is computationally attractive 
since candidates for v can be constructed from the columns of G. Namely, 
given any set of p — 1 independent columns of G, let v £ R p be a nonzero 
vector which is perpendicular to the p— 1 columns. To verify condition 4, 
we only need to check that the dot products of v and the remaining 
columns of G do not all have the same sign. It is left as an exercise to 
show that only values of v defined in this manner need to be considered. 
Note that the vector v , if it exists, defines the normal to a supporting 
hyperplane through the origin. 


Example 5.4. Using convexity to determine force-closure 

Consider the planar point contact grasps shown in Figure 5.8. In the 
first grasp, the contact locations are clustered near the corners of the 
rectangular object. The grasp map for these contacts is given by 


G = 


1 

0 


—a 


0 

-1 

b 


-1 0 
0 1 

—a b 
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Figure 5.8: Determining whether a planar grasp with point contacts is a 
force-closure grasp. 


where a,b > 0. A supporting hyperplane candidate can be formed by 
choosing any two columns of G. Let Vij represent the normal to the 
plane formed by Gi and Gj. Enumerating all possibilities, we have 


T 

v 12 = 

[a 

b 1 ] 

v t 12 G = 

[0 

0 

T 

V 13 = 

[o 

1 0] 

vIsG = 

[0 

-1 

T 

v 14 = 

[a 

- b 1 ] 

v[ 4 G = 

[0 

2b 

T 

v 23 ~ 

[a 

-6 “I] 

V23G = 

[2a 

0 

T 

V 2A = 

[1 

0 0] 

v\ T 4 g = 

[1 

0 

T 

^34 = 

[a 

b - 1 ] 

u 3 t 4 G = 

[2a 

- 


Since none of these possibilities satisfy condition 4 of Proposition 5.3, 
we conclude the grasp is force-closure. The convex hull for this grasp is 
shown in the upper right of Figure 5.8. 

For the second grasp, the contact points have been chosen such that 
a,b = 0, and hence 


G = 


1 

0 

0 


0 

-1 

0 


0 

1 

0 
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Figure 5.9: Force-closure for point contact with friction. 


This grasp is not force-closure since it is not possible to resist torques 
using positive linear combinations of the columns of G (condition 2). 
The convex hull of the columns of G lies completely in the f x , f y plane, 
and does not include a neighborhood of the origin (condition 3). Finally, 
choosing v = (0, 0,1), 

v t G = [0 0 0 0] , 
and hence condition 4 also fails. 


In some cases, the results of Proposition 5.3 can be extended to grasps 
which include frictional contacts. We motivate this with an example: 


Example 5.5. Using convexity for grasps with friction 

Consider the planar grasp shown in Figure 5.9. Rather than describe 
this grasp using our usual contact wrench basis, we note that all contact 
forces which lie in the friction cone can be written as positive linear 
combinations of the forces which describe the edges of the cone. Hence, 
we can determine if a grasp is force-closure by checking to see if the 
object wrenches corresponding to the edges of the contact friction cone 
positively span the object wrench space. 

In the case of planar contacts, this is equivalent to defining a contact 
map 


a = 


i 


1 

-M 


Hr —fir 


and a friction cone 


FC;={/eK 2 :/ 1 ,/ 2 >0}, 


which has the same form as two independent, frictionless point contacts. 
Hence, by appropriate choice of basis, we can use Proposition 5.3 to check 
force-closure of planar grasps with friction. 
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Figure 5.10: Approximations of a spatial friction cone. 


For spatial (3-D) grasps, the extension of Proposition 5.3 to contact 
models with friction is not so straightforward. Since the friction cone 
cannot be represented as the sum of a finite set of vectors, we cannot 
use the convex hull of a finite basis to determine if a grasp is force- 
closure. In practice, the circular friction cone can be approximated by a 
cone generated by a finite set of vectors. This situation is shown in Fig¬ 
ure 5.10. The force-closure condition can then be checked by evaluating 
the convex hull of the (conservative) approximation. If the convex hull 
contains a neighborhood of the origin, then the grasp is force-closure. For 
a finite-dimensional approximation, this is a sufficient but not necessary 
condition. 

Additional methods of checking force-closure for spatial grasps are 
given in the next section. 


4 Grasp Planning 

In this section, we briefly present methods for choosing contact locations 
which result in a force-closure grasp. 

4.1 Bounds on number of required contacts 

Suppose that we are given the task of designing a multifingered robot 
hand for use in a given set of tasks. One of the first decisions that 
must be made is the number and type of the fingers. In this section, we 
consider the following question: given a contact model, how many fingers 
are necessary to grasp any object? This places a lower bound on the 
number of fingers which we must include on our hand, given a choice of 
contact type. 

Given an object Ocl 3 (or Ocl 2 for the planar case), let E = dO 
denote the boundary of the object. We assume that E is a connected, 
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(a) (b) (c) 


Figure 5.11: Sets of vectors which positively span R 2 . 


piecewise smooth surface. Let A(£) denote the set of all wrenches that 
can be applied to an object using frictionless point contacts: 


A(£) 


n Ci 

Pc, x n Ci 


: Ci £ dO}, 


where n Ci is the inward pointing unit normal to £ at the point Cj, and p Ci 
is the contact point relative to the object frame. A surface £ is an excep¬ 
tional surface if the convex hull of A(£) does not contain a neighborhood 
of the origin. An object with an exceptional surface can never be grasped 
using only frictionless point contacts. Examples of exceptional surfaces 
are the sphere in R 3 and the circle in R 2 . 

For non-exceptional surfaces, upper and lower bounds for the number 
of contacts required for a force-closure grasp can be obtained by using 
two classical theorems in convex analysis. 

Theorem 5.4 (Caratheodory). If a set X = {iq, ■ • ■, Vk} positively spans 
R p , then k > p + 1. 


Theorem 5.5 (Steinitz). If S C R p and q £ int(coS')., then there exists 
X = {v\,... ,Vk} C S such that q £ int(coX) and k < 2 p. 

Carathcodory’s theorem implies that if a surface can be grasped with 
a force-closure grasp, then it must have at least p + 1 contacts. This is 
easy to visualize in the plane, as shown in Figure 5.11. In this case p = 2, 
and hence Caratheodory’s theorem asserts that any set of vectors which 
positively spans R 2 must have at least 3 elements. Suppose we choose 
any two of the vectors shown in the figure. If we label the two vectors as 
Vi and Vj, then —(Vi + Vj) never lies in the positive span of Vi and Vj. A 
similar argument holds for p > 2. Of course, a set of vectors may have 
more than p + 1 elements and still not positively span R p . For example, 
the set { 01 , 02 , 03 } in Figure 5.11a does not positively span R 2 . 

Steinitz’s theorem places an upper bound on the minimal number of 
contacts which are needed. Let S = A(£) and choose q = 0. If the 
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Table 5.3: Lower bounds on the number of fingers required to grasp an 
object. 


Space 

Object type 

Lower 

Upper 

FPC 

PCWF 

SF 

Planar 

Exceptional 

4 

6 

n/a 

3 

3 

(P = 3) 

Non-exceptional 



4 

3 

3 

Spatial 

Exceptional 

7 

12 

n/a 

4 

4 

(p = 6) 

Non-exceptional 



12 

4 

4 


Polyhedral 



7 

4 

4 


grasp is force-closure, then 0 £ int(S) and by Steinitz’s theorem there 
exist 2 p vectors iq,..., V 2 P G A(E) which generate a force-closure grasp. 
Hence, Steinitz’s theorem guarantees that we need at most 2 p contacts to 
grasp any non-exceptional object. These 2 p vectors may not be unique; 
Figures 5.11b and 5.11c show two choices of 4 vectors which span R 2 . 
Note that in Figure 5.11c, V 3 is redundant, but in Figure 5.11b all of the 
vectors are needed to positively span R 2 . 

Caratheodory’s and Steinitz’s theorems allow us to bound the num¬ 
ber of contacts required for a force-closure grasp using frictionless point 
contacts. If we let p be the dimension of the wrench space and k be the 
number of contacts, then 

p = 3 (planar) => 4 < k < 6 

p = 6 (spatial) => 7 < k < 12. 

It can be shown that for the planar case (p = 3) the lower bound is 
achievable for all non-exceptional surfaces. For the spatial case (p = 6), 
proofs exist for special classes of objects showing that the lower bound 
can be achieved, but the general case is not completely solved. Among 
the more useful classes of objects which can be grasped with at most 
seven contacts are the set of polyhedral objects and the set of everywhere 
smooth objects. 

For more general contact types, bounds on the number of contacts 
are also available. A detailed description of these bounds is beyond the 
scope of this book, but can be found in [66]. We note that for the point 
contact with friction case, the lower bounds can be exhibited using a 
regular tetrahedron (or equilateral triangle in the plane) and choosing 
p < tan 19.5°. This issue is explored further in the exercises and in the 
next section. 

Table 5.3 summarizes the number of contacts required for various con¬ 
tact models. These bounds are lower bounds on the number of contacts 
required for a force-closure grasp of any object in the indicated class. 
That is, in order to be able to grasp every object in the class, at least 
that many contacts must be available. 
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Figure 5.12: Geometric relationships for 2-D force-closure grasp using 
two point contacts with friction. 

4.2 Constructing force-closure grasps 

In this section, we show one particularly simple method of computing 
and constructing force-closure grasps for the case of two point contacts 
with friction and a planar object. The algorithm is based on the following 
theorem: 

Theorem 5.6. Planar antipodal grasps [82] 

A planar grasp with two point contacts with friction is force-closure if 
and only if the line connecting the contact point lies inside both friction 
cones. 

A grasp satisfying the conditions of this theorem is called an antipodal 
grasp. The proof of the theorem is left as an exercise. 

To convert this theorem into an algorithm for planning grasps, we 
note that the conditions of the theorem can be written as a set of linear 
inequality constraints. Let n\ denote the inward pointing normal at the 
contact point pi and n 2 denote the inward point normal at the contact 
point P 2 (see Figure 5.12). Given a unit vector n, define n ± to be the 
unit vector perpendicular to n which satisfies 

n®n^~ = + 1 , 

where <g) is the 2-D cross product: 

a (g> b = det[a b] a, 6sK 2 . 

Suppose that we are given a polygonal object and we wish to find 
all possible force-closure grasps. The constraints that the line between 
contacts lie in the relevant friction cones become: 

A: (m - pn\ j 1 ) ® (p 2 - Pi) > 0 

B : (m + pni ) <8> {jP 2 ~ Pi) < 0 

C : (n 2 — pn^) <S) (pi ~ P 2 ) > 0 

D : (n 2 + pn^) <S) {pi - P 2 ) < 0. 
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E2 


1.0 



0.5 


0 


A 


B 


Figure 5.13: Force-closure grasps for an equilateral triangle. The lightly 
shaded region corresponds to the set of all force-closure grasps and the 
dark square (and corresponding darkened regions on the triangle) indi¬ 
cates maximally independent regions of contact. 

Given any two edges, we can represent these inequalities on a graph, by 
drawing the lines which correspond to the inequalities becoming identi¬ 
cally zero. Figure 5.13 gives an example of such a graph. 

This graph gives a complete description of the force-closure grasps 
between any two edges. By enumerating all pairs of edges, we can gen¬ 
erate all possible force-closure grasps for the object. There are many 
methods which might be used to choose among the grasps. One is to 
assign a quality measure to a grasp and choose the grasp which maxi¬ 
mizes this quality measure and is also force-closure. Another possibility 
is to choose a grasp such that it is maximally distant from the edges of 
the force-closure region. Such a grasp is equivalent to choosing maximal 
independent regions of contact on each edge. That is, we can attempt 
to find regions on each edge such that if either contact is within the re¬ 
spective region, the grasp is force-closure. Such a region corresponds to 
a square in the force-closure grasp. 

The algorithm presented above can be extended to the spatial case 
using the following variant of Theorem 5.6: 

Theorem 5.7. Spatial antipodal grasps [82] 

A spatial grasp with two soft-finger contacts is force-closure if and only 
if the line connecting the contact point lies inside both friction cones. 

It is also possible to extend the algorithm sketched above to work 
for objects with curved surfaces. In this case, the constraints become 
nonlinear functions of the contact locations (suitably parameterized). 
However, the algorithm can still be applied if there is a way to find the 
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zeros of the given expressions. 


5 Grasp Constraints 

In the previous sections, we analyzed the grasp kinematics ignoring the 
kinematics of the fingers. That is, we assumed that forces could be applied 
at contact points irrespective of how those forces were generated. In this 
section, we extend our analysis to include the kinematics and statics of 
the robotic fingers. 

We shall view a robot hand grasping an object as a kinematically 
constrained system. In correspondence with the preceding sections, we 
assume that the contact locations are fixed on the object. In this case, the 
constraints between the object and a finger can be formulated by requiring 
that certain velocities are equal. For example, at a given contact point, 
the velocity of the fingertip and the velocity of the contact point (on the 
object) must agree in the direction normal to the surface. For simplicity, 
we assume throughout the remainder of this section that all contacts are 
either point contacts with friction or soft-finger contacts. This justifies 
our assumption that the contact location is fixed; the more general case 
is considered in the next section. 


5.1 Finger kinematics 

In Chapter 3 we derived the forward kinematics for an open-chain manip¬ 
ulator using the product of exponentials formula. Recall that the spatial 
velocity of the end-effector of the robot can be written as 

v 8 \ = r st (e)e, 


where Jf t (0) £ is the spatial Jacobian of the forward kinematics 

function. The twist V 8t is the generalized velocity of the tool frame, 
written with respect to a fixed base frame. The body velocity of the tool 
frame is given by 

It represents the instantaneous velocity of the tool frame, written in tool 
coordinates. 

We model a robot hand as a collection of robots (fingers) attached 
to a common base (palm). For each finger in the hand, attach a frame 
Si to the base of the finger and a frame F) to the fingertip at the point 
of contact, as shown in Figure 5.14. Note that the frame F, moves 
with the fingertip, while the frame C,, also located at the contact point, 
moves with the object. Let J 8 .f. be the Jacobian for F, relative to the 
fixed frame Si so that 


V* 

' Q 


fi 


= Ad~ 1 ,, ,J? 


SPsWu 


(5.8) 
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Figure 5.14: Grasp coordinate frames. 

where Of,. £ K ni is the vector of joint angles for the ith finger. 

The grasping constraint for a finger gives the directions in which mo¬ 
tion is not allowed. Relative to the contact frame Cj, these constraints 
can be written as constraints on the velocity of the finger frame Fi. For 
example, a frictionless point contact restricts the velocities of the finger 
and the object such that the relative velocity between these two frames is 
zero in the direction normal to the surface. By convention, this is the pr¬ 
axis direction in the contact frame, and hence we can write the constraint 
as 

[0 0 1 0 0 0 ] Vf. c . = 0 . 

Note that the matrix multiplying Vf. c . is precisely the transpose of the 
wrench basis for a frictionless point contact. 

In general, the directions in which motion is constrained are precisely 
those in which forces can be exerted. Hence, for a contact with wrench 
basis B Ci , we require that 

BlV} iCi = 0. (5.9) 

Equation (5.9) restricts the relative velocity between the finger and the 
object to be perpendicular to the directions in which forces can be applied. 

We can now proceed to write equation (5.9) in terms of a set of known 
quantities. We perform all computations using body velocities, and hence 
we temporarily drop the superscript b. Using the velocity relations in 
Chapter 2 Section 4, we can write Vf iCi as 

V fiCi = Ad" 1 V fiP + V pCi 

9pc \ (5.10) 

= — AdAd 9pfi V p f i + V pCi . 
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Vpfi is the velocity of the fingertip relative to the palm frame. Since the 
finger base frames are fixed relative to the palm frame, it follows that 


V pfi = Ad ' V ; „., + 14. f, = 14./,. 


9sif, 


(5.11) 


We can write V pCi by adding the velocity between the palm and object to 
the velocity between the object and contact. Since the contact frame is 
fixed relative to the object frame (by the fixed contact assumption), we 
have 

V pCi = Ad" 1 ^ V po + V OCi = Ad"^ y po . (5.12) 

Substituting equations (5.11) and (5.12) into equation (5.10) gives 

VfiCi = ' Ad g pCi Ad ffp/i V<>ifi + Ad s OCi Vpo- 


We can now substitute this into the constraint equation and use the finger 
kinematics to yield 



Ad 


9pfi 



J a . 


fi Si 


Bl Ad" 1 V m 


This equation represents the ith contact constraint in terms the finger 
joint angles 9 and the object configuration g po . 

The contact constraint can be simplified by making use of the adjoint 
equation 


Ad" 1 

ypci 


Ad 0 , Ad„ 1 

Upfi 9sifi 


= (Ad 9 


Ad ffpo Ad So 


,r‘ = Ad: 


-1 

9s iC 


Making this substitution, the constraint for the ith finger becomes 


B; 


' Ad j' j; tft (Of t )0f t = ( Ad L‘ B Ci ) T v b po 


= gT vt 


(5.13) 


Stacking equation (5.13) for each finger, we can write the constraint in 
matrix form. Let k be the number of fingers, rif the number of degrees 
of freedom for the ith finger, and set n = n\ + ■ ■ ■ + n^. Define the hand 
Jacobian as the matrix Jh(0,x o ) : R" —> R m given by 


Jh {9 , Xq) 


-l 


Bl Ad ,,, 




o 




where 9 = (9 fl ,..., 0 fk ) 
have the form 


Then, the constraints in equation 


J h (9 lXo )9 = G T V» 0 


(5.15) 
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where x 0 := g po is the configuration of the object and 8 is the vector 
of all finger joint angles. Equation (5.15) is the fundamental grasping 
constraint ; it relates velocities of the fingers to the velocity of the object. 
The quantities in the constraint plus a description of the grasp friction 
cone provide all of the necessary information for modeling a fixed contact 
grasp. 

Although we have derived the grasp constraint in a very mechanical 
fashion, it is possible to interpret equation (5.15) in a simple and mean¬ 
ingful way. Equation (5.15) equates the velocity of the object and the 
velocity of the fingertip at the point of contact between the two. Since 
motion in some directions may be allowed (for example, rotation about 
the contact point), we only constrain those directions specified by the 
columns of B Ci . Equation (5.15) is merely a restatement of equation (5.9) 
written in a more useful form. 

The quantities Jh , G, and FC completely characterize the proper¬ 
ties of a set of fingers grasping an object. This motivates the following 
definition. 

Definition 5.4. Representation of a multifingered grasp 

A multifingered grasp is described by the hand Jacobian Jh(8 , x 0 ) : R n —> 
M m , the grasp map G : R m —> R p , and the friction cone FC C which 
satisfies the properties in Definition 5.1. 

Formally, we distinguish between a grasp, which includes specification 
only of the object and the contact locations, and a multifingered grasp, 
which includes a description of the finger kinematics. When the usage is 
clear from context, we use the term grasp. 

5.2 Properties of a multifingered grasp 

We are now in a position to study the mathematical properties of the 
grasp constraint in equation (5.15) and interpret them. In the beginning 
of this chapter, we saw that the force-closure properties of a grasp are 
characterized by the grasp map G and the friction cone FC. In studying 
force-closure, we assumed that all allowable contact forces could be ap¬ 
plied by the fingers; we can now detail under what conditions this is true 
and what happens when this condition fails. 

A fundamental property of a multifingered grasp is the ability of the 
robot fingers to accommodate an object motion. If a set of fingers can 
accommodate any motion of the object without losing contact, we say 
that such a grasp is manipulable: 

Definition 5.5. Manipulable grasp 

A multifingered grasp is manipulable at a configuration (0, x 0 ) if for any 
object motion V^ Q there exists 8 which satisfies equation (5.15). 

The following proposition follows from the definition. 


237 


finger 


contact 


object 


velocity 

domain 



force 

domain 



Figure 5.15: Diagram of relationships for a multifingered grasp. The 
contact force must satisfy f c £ FC for these relationships to hold. 


Proposition 5.8. Characterization of all manipulable grasps 

A grasp is manipulable at a configuration ( 9 , x a ) if and only if 

K(G T )cK(J h (d,x 0 )). 

Manipulability does not require that the matrix be injective (one- 
to-one); there may be many finger motions which accommodate a given 
object motion. This can happen precisely in the case where Jh is full 
(row) rank with more columns than rows (n > m), and hence has a 
non-trivial null space. Any joint velocities 9 jv £ N(Jh) result in no 
motion of the contacts; we call 6^ an internal motion. Internal motions 
can be added to a given finger velocity without changing the velocity of 
the grasped object and so the finger-object system becomes a redundant 
manipulator. Note that this kinematic redundancy may appear even if 
none of the individual fingers are redundant, due to the B Ci terms in the 
definition of the hand Jacobian. These have the effect of masking out 
joint velocities in directions which do not violate the contact constraints, 
and hence allowing internal motions in those directions. 

Equation (5.15) describes the velocity relationships between the ob¬ 
ject and fingers. We will also make use of the force relationships. As 
described in Section 2, the grasp map characterizes the relationship be¬ 
tween contact forces and object wrenches: 

F 0 = Gf c . 

The contact forces can be related to the joint torques by using the trans¬ 
pose of the hand Jacobian: 

r = Jlf c (5.16) 

(this follows by equating the work done by the joints and contacts). As in 
our derivation of the grasp map, the contact forces must lie in the friction 
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Table 5.4: Grasp properties. 


Property 

Definition 

Description 

Force-closure 

Can resist any applied 
wrench 

G(FC) = 

Manipulable 

Can accommodate any 
object motion 

■R{G T ) C U(J h ) 

Internal forces 

Contact forces /n which 
cause no net object 
wrench 

f N £ A f(G) D int(FG) 

Internal motions 

Finger motions On 
which cause no object 
motion 

On £ N(Jh) 

Structural forces 

Object wrench Fj which 
causes no net joint 
torques 

G+Fj £ Af(J h T ) 


cone FC in order to avoid slipping. The complete set of relationships 
between the velocities and forces in a multifingered grasp are summarized 
in Figure 5.15. 

As in Chapter 3, some care must be taken in interpreting equa¬ 
tion (5.16) in the case that Jh £ M mxn i s not square. If the grasp is 
manipulable and force-closure, then it follows that Jh is surjective onto 
the range of G T . In this case, we can always exert a given contact force, 
but the joint torques required may not be unique. If internal motions are 
present, the dynamics of the manipulator must be taken into account. 
This is discussed more fully in the next chapter. 

If a grasp is not manipulable, then it may not be possible to exert 
arbitrary contact forces. In this case, J ^ will have a non-trivial null 
space and hence there may exist contact forces f c which give r = 0 in 
equation (5.16). This is completely analogous to the case in an open-chain 
manipulator with fewer joint degrees of freedom than the dimension of 
the workspace. In particular, we call contact forces which lie in the null 
space of J^ structurally dependent forces, since they generate forces in the 
mechanism that cannot be determined without more information about 
the elastic properties of the mechanism. 

As we have seen above, the properties of a grasp can be completely 
described by the grasp map G, the hand Jacobian Jh , and the friction 
cone FC. These properties of a grasp are summarized in Table 5.4. 
Note that force-closure and manipulability are separate properties. It is 
possible for a grasp to be force-closure but not manipulable, manipulable 
and not force-closure, or neither force-closure or manipulable. A few of 
these possibilities are illustrated in Figure 5.16. 
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not manipulable 



not force-closure 
manipulable 


not force-closure 
not manipulable 


Figure 5.16: Some grasps illustrating manipulability and force-closure 
properties. 


5.3 Example: Two SCARA fingers grasping a box 


As an example of how these calculations proceed, consider the grasp 
shown in Figure 5.17. In this example, two SCARA fingers are used to 
grasp a box. Assume that the contact points are located at p Ci = (0, ±r, 0) 
with respect to the object frame of reference, and that each contact is 
modeled as a soft-finger contact. 

The majority of the quantities needed to describe the grasping con¬ 
straint have been previously computed. The grasp map was derived in 
Example 5.2: 


rO 1001 0 0 On 

0 0100 0 -1 0 

1 0 0 0 0 1 0 0 

—r 0 0 0 0 +r 0 0 

0 0010 0 0-1 
_ 0 +r 0 0 —r 0 0 0 - 


using the contact frames shown in Figure 5.5. The wrench basis for each 
finger was given by 

"1 0 0 0 " 

0100 
d _ 0010 

— oooo • 
oooo 
Lo o o l J 

For simplicity, we take the length of the fingertips as 1% = 0 and hence the 
contact occurs at the usual SCARA end-effector location. In this case, 
the Jacobian is that given in Example 3.8: 


0/1 COS 01 li COS 01+^2 COs(0i+02) 0 “ 

OZisin0i Zi sin 01 +Z 2 sin( 0 i+ 02 ) 0 
0 0 0 1 . 

0 0 0 0 

0 0 0 0 

11 1 0 J 

If I 3 ^ 0, then we must recompute the Jacobian to include the additional 
displacement (see Exercise 11). 

The only remaining quantity to calculate is Ad” 1 , which maps twists 

J s i c i 

from the spatial frame into the contact frame for a given finger. We can 
construct Ad~ c using the transformation 



(g SiP 9po 9oa ) 


-1 


9oci 9po 


9 


-l 

SiP' 
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Figure 5.17: Two-fingered grasp using SCARA robots. 


Since the transformations g OCi and g SiP are constant, we can write 





Ad 


-l 

Qs^p 


Note that only Adj^ 1 depends on x D = g po , the current configuration of 
the object, and the other transformations are constant matrices. 

To construct the grasp constraint, we compose the matrices given 
above in the appropriate fashion. In full generality, the contact con¬ 
straints are quite complex and the calculations are more suited for auto¬ 
mated rather than manual computation. We therefore restrict ourselves 
to analyzing the grasp constraints for the object configuration shown in 
Figure 5.17. That is, we set 


R P o — I 


Ppo - 


0 

0 

a 


and hence Ad,^ 1 _ becomes a constant matrix. Rather than compute it 
in pieces, we use the transformations g Si cii which can be written down 
by inspection: 


R-sia — 
PsiCl — 


0 10 
0 0 1 
10 0 

0 

b—r 


Ad" 1 = 

9s 1 c 1 


R 


S 1 Cl 

0 


/ b—r 0 0 \ 

I 0 a i—b ) 

V -a 0 0 / 

dT 
V«i r-t 


Rs2C2 - 


PS2C2 


1 0 0 
0 0-1 
0 1 0 
0 

—fe+r 


Ad" 1 = 

i/s 2 c 2 


R 


S2C2 

0 


/ 0 a b—r \ 
I r—b 0 0 ) 

V a 0 0 / 


Ri 
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Finally, we calculate Jh- 


A = 


'BT A d" 1 J* t (Or ) 

Cl Qs-\C4 Sl/l' J 1 ' 

0 


Jll 

0 ' 


0 

j s s 2 / 2 (^). 


0 

10 


O 

O 

0 


1 ' 



Jn — 


J22 — 


-b + r —b + r + hci — b + r + l\C\ + l 2 c \2 0 

0 W S 1 W S 1 + ^2 S 12 0 

00 00 

b - r b - r + I 3 C 3 b — r + I 3 C 3 + I 4 C 34 0 

0 0 0 1 

0 —I 3 S 3 —I 3 S 3 ~ I 4 S 34 0 

0 0 0 0 


(for the particular configuration shown in Figure 5.17). 

We can now evaluate the properties of the grasp. From Example 5.2 
we have that the grasp is force-closure (this does not depend on the fingers 
which actually exert the forces). However, the grasp is not manipulable, 
since rotation of the object about the y-axis yields 


G t 




0 1 

" 0 " 


0 

0 


0 

0 


1 

0 


0 

1 


0 

. 0 . 


0 



-1 


i n{j h ). 


The grasp contains internal motions which are in the vector space spanned 

by 


rOi 


rO-1 

0 


0 

1 


0 

0 

0 

II 

s? 

3 S 

0 

0 

0 


0 

0 


1 

L o J 


L o J 


at the configuration shown. These motions correspond to rotating the 
last revolute joint of each finger, resulting in a rotation of the fingertip 
about contact point. In the more general case where I 3 ^ 0 and the hand 
is in an arbitrary configuration, the internal motion still exists but all 
three revolute joints will have nonzero instantaneous velocities. 


6 Rolling Contact Kinematics 

Most real-world grasping situations involve moving rather than fixed con¬ 
tacts. Human fingers and many robotic fingers are actually surfaces, and 
manipulation of an object by a set of fingers involves rolling of the fin¬ 
gers along the object surface. In this section, we derive the kinematic 
equations for one object rolling against another and extend the grasping 
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Figure 5.18: Surface chart for a two-dimensional object in R 3 . 


formulation to moving contacts. The formulation here covers rolling be¬ 
tween smooth fingers and smooth objects. It does not cover scenarios in 
which fingers roll on the edge of an object, but extensions to this case are 
possible. We begin with a brief description of surface parameterizations. 

6.1 Surface models 

Given an object in M 3 , we describe the surface of the object using a local 
coordinate chart , c : U Cl 2 ^ R 3 , as shown in Figure 5.18. The map c 
takes a point (u,v) £ R 2 to a point x £ R 3 on the surface of the object, 
written in the object frame of reference, O. Thus, locally, we can describe 
a point on the surface by specifying the corresponding (u, v). In general, 
it may take several coordinate charts to completely describe the surface 
of the object. A surface S is regular if for each point p £ S there exists 
a neighborhood V C R 3 , an open set U C R 2 , and a map c : U —> V fl S 
such that 

1. c is differentiable. 

2. c is a homeomorphism from U to V fl S. That is, c is continuous, 
bijective (one-to-one and onto), and has a continuous inverse. 

3. For every a = (u,v) £ U, the map §§(a) : R 2 — > R 3 is injective 
(one-to-one). 

At any point on the object, we can define a tangent plane which 
consists of the space of all vectors which are tangent to the surface of 
the object at that point. The tangent plane is spanned by the vectors 
c u := and c v := That is, any vector which is tangent to the 
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surface at a point c(u, v) may be expressed as a linear combination of the 
vectors c u and c„, evaluated at (u, v). A coordinate chart is an orthogonal 
coordinate chart if c u and c v are orthogonal. 


Theorem 5.9. Locally, there exists an orthogonal chart for all regular 
surfaces. 

The proof can be found in any standard book on differential geometry. 

In the sequel, we will make frequent use of some more detailed con¬ 
cepts from differential geometry. Although the application of the rolling 
equations does not require knowledge of these concepts, they do allow a 
deeper understanding of the material. We present here a brief review of 
the relevant topics; a full description can be found in DoCarmo [27]. 

In order to define the area of a surface, one needs to define the inner 
product between two tangent vectors on the surface. This defines the area 
of a parallelogram and the total area can then be calculated by integrating 
the infinitesimal areas generated by parallelogram-shaped patches on the 
surface. The first fundamental form for a surface describes how the inner 
product of two tangent vectors is related to the natural inner product 
on R 3 . In a local coordinate chart, it is represented by a quadratic form 
I p : R 2 x R 2 —> R which takes two tangent vectors attached at a point 
p = c(u, v ) and gives their inner product. If c is a local parameterization, 
then the matrix representation of the quadratic form is given by 


Ip — 


■ T 
C u C u 
T 

p v Cu 


T 

C u C v 

T 

Cy c v 


(5.17) 


We will use the symbol I p to represent both the quadratic form and its 
matrix representation. Note that each element of I p is an inner product 
between vectors in R 3 and that I p is symmetric and positive definite. If 
a parameterization is orthogonal, only the diagonal terms are nonzero. 

The first fundamental form can be used to define the metric tensor 
for a surface. The metric tensor is given by the square root of the first 
fundamental form and is used to normalize tangent vectors. We define 
the matrix M p : R 2 —> R 2 as the positive definite matrix which satisfies 

Ip = M p ■ M p . 


In the case that the parametrization is orthogonal, M p has the form 


M p = 




(5.18) 


(The metric tensor described here is the square root of the Riemannian 
metric used in differential geometry.) 

At each point on a surface S , we can define an outward pointing unit 
normal by taking the cross product between the vectors that define the 
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tangent space. We identify the set of all unit vectors in R 3 with § 2 , the 
unit sphere in R 3 . The Gauss map N : S —> § 2 gives the unit normal at 
each point on the surface S. In local coordinates, 


N(u , v ) 


C U X Cy 
| C-U X Cy | 


(5.19) 


For smooth, orientablc surfaces, the Gauss map is a well defined, differ¬ 
entiable mapping. We write n = N(u, v) for the unit normal at a point 
on the surface. 

The directional derivative of the Gauss map defines the second funda¬ 
mental form for a surface. The second fundamental form is a measure of 
the curvature of a surface. In a local coordinate chart, it is represented 
by a map II p : M 2 x R 2 —> R: which has a matrix representation 


Up 

— 

c^n u 

T 

Cy n v 


L c v n u 

Cy n v 

dn 

du 

and n v 

_ dn 

dv 


(5.20) 


where p = c(u,v), n u := and n v := This matrix describes the 
rate of change of the normal vector projected onto the tangent plane. It 
may be interpreted as follows: if p(s) € S is a curve lying on S that is pa¬ 
rameterized by arc length and whose tangent vector is p' , then (p') T U p p' 
gives the usual curvature of the spatial curve p. 

For our purposes, it will be convenient to scale the second fundamental 
form and define the curvature tensor for a surface. For an orthogonal set 


of coordinates, the curvature tensor is a mapping K 
as 


K P = M~ t U p M~ 


T 

Co, TLy 


T 

Co, n v 


T 

Co, n v 


T 

Co, n v 


R 2 —> R 2 defined 


(5.21) 


The factors of M” 1 are normalization factors which account for scaling 
present in the local coordinate chart for the surface. 

The curvature tensor can also be computed in terms of a special coor¬ 
dinate frame called the normalized Gauss frame. If c(u, v ) is an orthog¬ 
onal chart, then we define the normalized Gauss frame as 


[x y z] 




(5.22) 


The normalized Gauss frame provides an orthonormal frame at each point 
on the surface. In terms of this frame, the curvature tensor is given by 


K P = 



n u 

IM| 


n v 

IMI > 


(5.23) 


which can be interpreted as a measure of how the unit normal varies across 
the surface, as projected on the tangent plane. Again, a normalization 
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Figure 5.19: The contact frame is a moving frame along a curve pit). 


factor is present to account for scaling due to the parameterization. If 
the surface is flat, then n u = n v = 0 and K p = 0. 

Finally, we define the torsion form. For a curve, the torsion measures 
the rate of change of curvature along the curve. The torsion of a surface is 
a measure of how the Gauss frame twists as we move across the surface, 
again projected onto the tangent plane. To compute the torsion, we 
only need to keep track of how either x or y changes, since they are 
orthonormal. We define the torsion form T : R 2 —> R as 


x u x v 


c*c uu 

c? c uv 

II Cull ||C„|| 


UkuPIM 

IMlkpJ 


(5.24) 


where x u and x v denote the partial derivative of x with respect to u and v, 
c U u '■= and c uv := g u £ v . Note that the torsion form is represented as 
a row vector, not a matrix. The torsion form is related to the Christoffcl 
symbols for a surface parameterization (see [27]). If a surface is flat, then 
T p = 0. 

Given a parameterization, (M p , K p ,T p ) are collectively referred to as 
the geometric parameters of the surface. These parameters describe the 
local geometry of the surface and play an important role in the kinematics 
of contact which we will pursue in the section that follows. 

Let p(t) € S be a curve on the surface of the object and define the 
contact frame along the curve to be the frame C which coincides with 
the Gauss frame at time t (see Figure 5.19). 2 We wish to determine the 
motion of the contact frame as a function of the geometric parameters 
and the velocity of the curve. If we fix a frame O in the object then the 
motion of the frame C is given by the rigid transformation g oc {t) & SE( 3). 
We assume p(t) lies in a single coordinate chart c : U —> R 3 and we let 
a{t) = c -1 (p(t)) represent the local coordinates. 


Lemma 5.10. Induced velocity of the contact frame 

The (body) velocity of the contact frame C relative to the reference frame 

2 This is different from our previous convention, where we choose the 2-axis along 

the inward pointing normal. 
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(5.25) 


O of the object is given by V^ c = (v oc , lo oc ) where 


Vnc = 


Ma 

0 


^ OC — 

1 

£ o 

—oj z 

0 

Uy 


0 —TMa 

TMa 0 

KMa 


. -“v 


0 


~(KMa) T 

0 


(5.26) 

and M, K, and T are the geometric parameters of the surface relative to 
the coordinate chart ( c,U ). 

Proof. The position and orientation of the contact frame relative to the 
reference frame are given by 

Poc = Pit) = c(a{t)) 

Roc = [x(t) y(t) z{t )] = Ijf^rr TI^TT I \ZTd\\ ■ 

The translational component of the body velocity is given by v oc = R^ c p oc 
which yields 


r T 1 

X 

T 

dc . 

r T 

X 1 c u 

T 

T 

X C v 
T 


Ma 

y 

JT 

da U ~ 

y c u 

y c v 

T „ 

a = 

0 

Z 


\_ z C u 

Z Cy 




To show equation (5.26), we compute the body angular velocity: 



r t i 
x 1 



0 x T y x T z 

^ oc — — 

T 

y 

[* 

y z\ = 

y T x 0 y T z 


i 

h 

_i 


z T x z T y 0 


where the zero entries follow because x, y, and z are unit vectors. Now 
using the definitions of K , T, and M we have 


^oc — 


0 -TMa 

KMa 

1 M a 0 

-(KMa) T 

0 


□ 

Example 5.6. Geometric parameters of a sphere in M 3 

A coordinate chart for the sphere of radius p can be obtained by us¬ 
ing spherical coordinates, as shown in Figure 5.20. For the hemisphere 
centered about the a;-axis, we have 


c(u, v) = 


p cos u cos v 
p cos u sin v 
psmu 
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Figure 5.20: Spherical coordinate chart for a sphere. 


with 

U = {(it, v) : —7r/2 < u < 7t/2, —7t < v < 7r}. 

The partial derivatives of c with respect to u and v are given by 



—p sin u cos v 


—pcos usinv 

Cu — 

—psinusinv 

Cv — 

p cos u cos V 


p cos u 


0 


and we see that cjc„ = 0 and hence the chart is orthogonal. The curva¬ 
ture, torsion, and metric tensors are: 


' i /p 

0 ' 

M — 

P 0 

0 

l /p. 

1V1 — 

0 p cos u 


T = [0 — 1/p tan it] . 


6.2 Contact kinematics 

Consider two objects with surfaces S a and Sf which are touching at a 
point, as shown in Figure 5.21. We are interested in the motion of the 
points of contact across the surfaces of the objects in response to a relative 
motion of the objects. 

Let p 0 (t) £ S 0 and Pf(t) £ Sf be the positions at time t of the point 
of contact relative to two body-fixed frames O and F, respectively. For 
simplicity, we will restrict ourselves to the case where motion is contained 
in a single coordinate chart for each object. Let (c D , U 0 ) and (c/, Uf) be 
charts for the two surfaces, and a 0 = c~ 1 {p 0 ) £ U 0 and a/ = cj 1 (pf) £ 
Uf be local coordinates. We will assume that c 0 and c/ are orthogonal 
representations of the surfaces. Let ip be the angle of contact, defined as 
the angle between the tangent vectors and . We choose the sign of 

ip so that a rotation of through an angle ip around the outward normal 
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Figure 5.21: Motion of two objects in contact. 


of S 0 aligns with (see Figure 5.21). Collecting the quantities 
which describe the contact, we call 77 = (qj, a 0 , ip) the contact coordinates 
for Sf and S 0 . 

Let g 0 f £ SE(3) describe the relative position and orientation of Sf 
with respect to S 0 • We wish to study the relationship between g Q f and the 
local contact coordinates. To do so, we assume that g 0 f £ W C SE( 3), 
where W is the set of all relative positions and orientations for which the 
two objects are in contact. 

The coordinate charts (c D , U 0 ) and ( Cf,Uf) induce a normalized Gauss 
frame at all points in c 0 {U 0 ) C S 0 and Cf(Uf) C Sf. We define the 
contact frames C 0 and Cf as the coordinate frames that coincide with 
the normalized Gauss frame at p 0 {t) and Pf(t), for all t £ I, where / is 
the interval of interest. We also define a continuous family of coordinate 
frames, two for each r £ J, as follows. Let the local frames L 0 (r ) and 
L/(r), be the coordinate frames fixed relative to O and F, respectively, 
that coincide at time t = t with the normalized Gauss frame at p 0 (t) and 
Pf(t) (see Figure 5.22). 

We describe the motion of O and F at time t using local coordinate 
frames L 0 (t ) and Lf(t). Let vij f = (v x ,v y ,v z ) be the components of the 
(body) translational velocity of Lf(t) relative to L a (t) at time t. Sim¬ 
ilarly, let = (u x ,ijjy,Lo z ) be the (body) rotational velocity. Here 

(■ uj x ,uj y ) are the rolling velocities along the tangent plane at the point 
of contact, and u> z is the rotational velocity about the contact normal. 
Likewise, ( v x ,v y ) are the linear velocities along the tangent plane, i.e., 
the sliding velocities , and v z is the linear velocity in the direction con¬ 
tact normal. As long as the two bodies remain in contact, v z = 0. In 
addition, for pure rolling contact we have v x = v y = 0 and uj z = 0 and 
for pure sliding contact we have u x = u> y = u z =0. Since the local 
frames are fixed relative to their respective frame of reference, according 
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Figure 5.22: Relation between the one-parameter family of local frames, 
Lf(t), and the contact frame, Cf. At time t € /, Lf(t ) coincides with 
Cf- 


to Proposition 2.15 we have Vij, = Ad -1 V a f or, in components 


v° 

v u f - 


V x 

Vy 

V z 


W x 

LOy 


U) z 


Rji f v °s rtfi f p 0 u 0 f 

rtf i f wo f 


(5.27) 


We also let 

Rif = 


cos ip — sin ip 
— sin ip — cos ip 


and K a = R^K 0 R^. 


Note that R^ is the orientation of the x- and y -axes of Cf relative to 
the x- and y -axes of C 0 . Thus, K 0 is the curvature of O at the point of 
contact relative to the x- and y-axes of Cf. We call Kf + K a the relative 
curvature form. 


Theorem 5.11. Kinematic equations of contact [72] 

The motion of the contact coordinates, rj, as a function of the relative 
motion is given by 


a f = Mj\K f + k 0 )- 1 

d 0 = M-'R^Kf + Ko)- 1 ( 

\lw x 

ip = <x z + TfMfCtf + T 0 M 0 a 0 
0 = v z , 


— K 0 


+ Kf 


(5.28) 


where {v x ,v y ,v z ,u) x ,u> y ,uj z ) = V t b olf = Ad g ^V b f . 
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Before presenting the proof of the theorem, let us note that the equa¬ 
tions of contact only make sense when the relative curvature is nonsingu¬ 
lar. An example of a singular relative curvature occurs when one object 
is concave and the other is convex, and both have the same radius of cur¬ 
vature. In this case, small motions of the object can cause large motions 
of the contact and continuity is lost. To avoid this possibility, we shall 
assume that all manipulation occurs in an open set on which the relative 
curvature is invertible. 

Proof of theorem. We perform all calculations using body velocities and 
hence temporarily drop the superscript b. Since the frame Lf{t ) is fixed 
relative to the frame F, the velocity of Lf(t ) relative to F is given by 
Vfi f = 0. Therefore, using the velocity relationships from Chapter 2, 

v fc s = Ad~) c V fl} + V lfCJ = V lfCf . (5.29) 

Similarly, we find that 

V OCo = Ad-* co V olo + V loCo = V loCo . (5.30) 

We now compute the velocity of Cf relative to L a (t ) via two intermediate 
frames, namely Lf(t) and C 0 . 

At time t, the position and orientation of Cf relative to Lf(t ) are 
Pi f c f = 0 and R ifCf = I. Thus, 


Vioc f = V Uf + V) 


fCf 


and since p Co c f = 0 


V loCf = 


[R l oCf 0 

0 r t 


■o Cf. 


VloCo T Vc oC j . 


Combining equations (5.29) through (5.32) yields 


(5.31) 


(5.32) 


v Uf + v fCf 


R 


T 

CoCf 


0 


0 


R 


T 

CoCf 


Voc 0 + V CoCf 


(5.33) 


We now find the values of each of the quantities in equation (5.33) in 
terms of the geometric parameters and motion parameters. First, we 
observe that 


Pc 0 Cf — 0 


R 


Co Cf 


Rxfj 0 

0 1 


11 CoCf — 0 


^C Q Cf 


0 l 

o 

.Tp. 


(5.34) 


and, by definition, Vij f = {v x ,v y ,v z ,ui x ,ui y ,u z ). 
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Second, according to Lemma 5.10, 


and 


v f c f ~ 


M f df 

0 


u f c t — 

0 — Tf MfQ, f 

TfMfdf 0 

K f M fdf 

-(K f Mfdf) T 

0 

Voc„ = 

M OCq = 

M 0 d 0 

0 

0 —T 0 M 0 d 0 

T 0 M 0 d 0 0 

K 0 M 0 d 0 


— {K 0 M 0 d 0 ) T 

0 


(5.35) 


(5.36) 


Substituting equations (5.34) through (5.36) into equation (5.33) and 
equating components, we get 


Mfdf 

0 


v x 

Vy 

v z 


Ri/jModo 

0 


KfMfdf 

+ 

UJy 

. 

1 

. o o 

_1 


Rip Ko M 0 a 0 

_ TfMfdf _ 


UJ Z 




T 0 M 0 d 0 


(5.37) 


After some algebraic manipulation, we can write equation (5.37) in the 
form given by equation (5.28). jj 

A common situation in grasping is to assume that the fingers roll 
without slipping on the object. In this case we can simplify the contact 
kinematics. 

Corollary 5.11.1. The contact coordinates for rolling contact evolve 
according to 

df = Mj\K f +K 0 )- 1 

d 0 = M~ 1 R^,{Kf + K 0 ) 
ip = TfMfdf + T 0 M 0 d 0 . 

Example 5.7. Sphere rolling on a plane 

Consider a spherical finger of radius p rolling on a plane, as shown in 
Figure 5.23. The local coordinates of the plane are chosen to be c 0 (u, v ) = 


^X 


-1 


(5.38) 
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Figure 5.23: Spherical finger rolling on a plane. The finger is only allowed 
to roll on the object and not slip or twist. 


(u,v, 0). The sphere requires multiple coordinate charts to describe the 
entire surface, so we shall restrict ourselves to the chart 


c f (u,v) 


p COS U COS V 

—pcosusinu 
psinu 


where p is the radius of the sphere and —7r/2 < u < 7r/2, — 7r < v < tt. 
The curvature, torsion, and metric tensors are: 


M 0 = 
T 0 = 


'0 

o' 

K f = 

0 

0 

1 

o' 

Mf = 

0 

1 

[0 

0] 

Tf= [ 


l /P 

0 

P 

0 


0 

l /P. 

0 

p COS U f 

— 1/p tanu 


/]• 


The equations governing the evolution of the contact point are 




0 


-1 

Vf 


sec Uf 


0 

ilo 

= 

— p sin'll) 

U! x + 

—p cos ip 

Vo 


—p cosi/j 


psinij] 



— tan Uf 


0 


6.3 Grasp kinematics with rolling 

We are now in a position to describe the grasp kinematics when the 
fingers are allowed to roll and possibly slide on the object (depending on 
the contact model). Figure 5.24 shows the coordinate frames which we 
shall use. We assume that the finger and object shapes are completely 
known and that local, orthogonal, surface parameterizations are available. 
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Figure 5.24: Grasp coordinate frames for moving contacts. 


Abstractly, the derivation of the grasp kinematics is identical to the 
fixed contact case except that the grasp map and hand Jacobian depend 
on the instantaneous contact location, which in turn depends on ryj, the 
contact coordinates for each finger. Thus, the contact kinematics have 
the form 

J h {0,x o ,r])9 = G T {rf)Vp 0 
Vi = A i(Vi)Vo fi , 

where g = (rji,... ,r/k) £ R 5fc is the vector of contact coordinates and 
A i{Vi) £ R 5x6 encodes the contact kinematics for each finger, as given 
in equation (5.28). One additional fact is needed to extend the previous 
analysis to the moving contact case: BJVq C . = 0. This condition is a 
statement that the contact point is not allowed to move in any of the 
constrained directions. A complete derivation is left as an exercise. 

Equation (5.40) describes the grasp kinematics in terms of a set of 
ordinary differential equations. To analyze the properties of the grasp, 
we make use of the following result. Let W C SE( 3) be the set of all 
configurations g 0 f for which the two objects are in contact. 

Proposition 5.12. Smooth dependence of r) on g a f 

There is a smooth local bijection between g £ R 5 and g Q f £ W C SE( 3) 
if the matrix 

Kf + R^K 0 Rlp 

is full rank. 

The proof of this proposition requires application of the implicit function 
theorem to the mapping ry i—> g a f £ W and can be found in [77]. 
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Since gt is a smooth function of g 0 f = g P o9pfi and g P fi is a smooth 
function of 9, we can write this constraint as 

J h (9,x o )e = G T (0,x o )V» o . (5.41) 

where x a := g po . Equation (5.41) is a direct extension of equation (5.15) 
to the moving contact case. All of the definitions and properties which 
held for fixed contact grasps also hold for moving contact grasps. 

For most applications, finding g as a function of g 0 f is not computa¬ 
tionally feasible and we must use g directly. To make this explicit, let 
gi = (a/ 4 , a 0i , i/>j) represent the contact coordinates for the ith contact. 
In order to calculate Jh and G, we must compute the transformations 
Ad” 1 and Ad T -i, which are the only quantities that vary with g. In 

y s i c i 9° c i 

fact, since 


Ad~ 


= (Ad ff 


Ad Spo A dg OCi 


)" 1 =Ad : 


-1 

9oCj 


Ad ,” 1 Ad ” 1 


the only new quantity which really needs to be calculated is Ad^ 1 . 

Without loss of generality, we assume that the contact frame is aligned 
with the normalized Gauss frame for the object and hence 


where n 0i is the outward pointing surface normal. Furthermore, p OCi is 
just the contact location c 0i (oti) and hence 


dC 0 . , r _1 

n °i 

1 

jr 

o" 

1 

o 

1 


(5.42) 


Finally, although in principle the contact coordinates can be obtained 
by integrating the contact kinematics, in practice it is much more likely 
that one would need to measure each gi directly in order to maintain 
accuracy (see, for example, [7]). These and other implementation details 
do not alter the overall structure and properties of the grasp, but can 
have a dramatic effect on the performance of the system. 
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7 Summary 


The following are the key concepts covered in this chapter: 


1. A contact is described by a mapping between forces exerted by a 
finger at a point on the object and the resultant wrenches in some 
object reference frame. The contact basis B Ci : R TOi —> R p describes 
the set of wrenches that can be exerted by the finger, written in the 
contact coordinate frame. For contacts with friction, the friction 
cone FC Ci C R mi models the range of allowable contact forces that 
can be applied. The friction cone satisfies the following properties: 


(a) FC Ci is a closed subset of R mi with non-empty interior. 

(b) f\ifi £ FC Ci => afi + j3f 2 £ FC Ci for a, (3 > 0. 


2 . 


A grasp is a collection of fingers which exert forces on an object. 
The net object wrench is determined from the individual contact 
forces by the relationship F 0 = Gf c , where G £ R pxm is the grasp 
map: 



Cl 




Ad T -i : R p —> R p is a the wrench transformation between the 

9o H 

object and contact coordinate frames. The contact forces must all 
lie within the friction cone FC = FC Cl x • • • x FC Ck . 


3. A grasp is force-closure when finger forces lying in the friction cone 
span the space of object wrenches 


G(FC) = R p . 


A grasp is force-closure if and only if the grasp map is surjective 
and there exists an internal force /jv which satisfies G//v = 0 and 
f N £ int {FC). 

4. The fundamental grasp constraint describes the relationship be¬ 
tween finger velocity and object velocity: 

J h (9,x 0 )9 = G t (0,x o )x o , 


where 9 £ R" is the vector of finger joint angles and x 0 '■= g po is the 
configuration of the object frame relative to the palm frame. The 
hand Jacobian Jh £ R mxra is defined as 


Jh = 


Bj Ad” 1 JtAOfi) 


B c k Ad s s lc fe JS s k f k 


(Of* 
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where J|.y. is the spatial Jacobian for the ith finger and Ad^ 1 ^ is 
the twist transformation between the base and contact frames. For 
contacts in which rolling does not occur, G is a constant matrix. 

5. The relationships between the forces and velocities in a multifin¬ 
gered grasp are summarized in the following diagram: 


velocity 

domain 


force 

domain 


6. A grasp is manipulable when arbitrary motions can be generated 
by the fingers 

k(g t ) c n(j h ). 

A force-closure grasp is manipulable if and only if Jh is surjective. 

7. The contact kinematics describe how the contact points move along 
the surface of the fingers and object. For an individual rolling 
contact, the contact kinematics are 

a f = Mj\K f +K 0 )- 1 

d 0 = M~ 1 R^ J (Kf + K 0 ) 
ip = TfMfdf + T 0 M 0 d 0 . 

where (Mj, Ki, Tp) are the geometric parameters for a given coordi¬ 
nate chart on the surface. The contact kinematics allow G and Jh 
to be computed using r) = ( ctf,a 0 ,ip ) rather than solving for r) in 
terms of g po . 
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9 Exercises 


1. Construct the grasp map for the grasps shown below and determine 
if the grasp is force-closure. Assume that all contacts are frictionless 
point contacts. 



(a) (b) (c) 


2. Construct the grasp map for the grasps shown below and write 
the friction cone conditions with respect to the contact basis you 
choose. Determine if the grasp is force-closure. Assume that all 
contacts are point contacts with friction. 



3. Construct the grasp map for the grasps shown below and write 
the friction cone conditions with respect to the contact basis you 
choose. Determine if the grasp is force-closure. Assume that all 
contacts are soft-finger contacts. 



p, = tan 15° 

7 > 0 
(a) 



H > 0 

7 > 0 

(b) 


4. Additional contact models 
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Derive the wrench basis and friction cone for the contact models 
shown below. Assume the coefficient of sliding friction is /x > 0. 




(a) (b) 

5. Verify the lower bounds given in Table 5.3 by constructing the fol¬ 
lowing objects. Assume that vertex contacts are not allowed. 

(a) Construct a 2-D object which cannot be grasped by two point- 
contact-with-friction fingers and /x = tan 30°. Prove explicitly 
that the object cannot be grasped. 

(b) Construct a 3-D object which cannot be grasped by three 
point-contact-with-friction fingers and /x = tan 15°. Prove ex¬ 
plicitly that the object cannot be grasped. 

(c) True or false: For any convex 3-D object, a force-closure grasp 
can be constructed using at most three soft-finger contacts at 
smooth points (i.e., no vertex contacts). Prove or construct a 
counterexample. 

(d) (contributed by J. Canny). Prove that for any 3-D object with 
smooth boundary (not just piecewise smooth), a force-closure 
grasp can be constructed using at most three point contacts 
with friction or two soft-finger contacts. 

6. When controlling a multifingered robot hand, it is important to 
insure that desired contact forces lie strictly inside the friction cone 
to avoid slip. That is, we require that for any F 0 . there exists f c 
such that F a = Gf c and f c £ int(FC). Such a grasp is said to 
be prehensile. Let FC be the friction cone defined in Section 2. 
Show that if G{FC) = R p , then G(int(FC)) = G(FC) using the 
following steps: 

(a) Show that F £ int(FG) => aF £ int (FC) for all a > 0. 

(b) Show F £ int(FG), F' £ FC => F + F' £ int(FG). 
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(c) Show that there exists Jn £ int(EC') such that Gfw = 0. 

(d) Complete the proof by showing that G{FC) C G(int(FC)). 

Hence, a grasp is prehensile if and only if the grasp is force-closure. 

7. Prove Theorem 5.6: Show that a planar grasp with two point con¬ 
tacts with friction is force-closure if and only if the line connecting 
the contact point lies inside both friction cones. 


8 . For the objects given below, find all force-closure grasps using two 
contacts with friction. On each the objects, draw the independent 
contact regions corresponding to a maximal square contained in a 
force-closure region. Assume that the coefficient of friction for all 
contacts is fi = tan 45°. 




(a) (b) (c) 

9. Find all the force-closure grasps for two point contacts with friction 
grasping a circle. Express your answer as a sketch of the force- 
closure regions versus the two finger locations on the circle. Indicate 
on the circle a set of independent contact regions corresponding to 
a maximal square contained in the force-closure region. 


10. Derive the contact constraints for the hands shown below. De¬ 
termine if the grasps are force-closure and/or manipulable at the 
configuration shown. 




(a) 


(b) 


11 . Calculate the contact constraints for the grasp shown in Figure 5.17 
with Z 3 > 0 . 


261 













12. Consider the two-fingered grasp shown in Figure 5.17. Equate the 
locations of the fingertips with the contact locations on the box. 
Differentiate this algebraic constraint and show that it is equivalent 
to the answer given in the example at the end of Section 5. 

13. Give an example of two surfaces in contact which has singular rel¬ 
ative curvature form. 

14. Calculate the geometric parameters for an ellipsoid, a parabaloid, 
and a torus. 

15. The figure below shows an elliptic fingertip in contact with a flat 
object. The principal axes of the ellipse have length a, b , and c, 
respectively. 



(a) Give an orthonormal coordinate chart for the fingertip around 
the point of contact as shown in the figure. 

(b) Assume the fingertip is in rolling contact with the object. De¬ 
rive the equations of contact. 

(c) Compute the velocity of the fingertip relative to the object 
which satisfies rolling constraint and produces a contact ve¬ 
locity of d 0 = (0,!)),v€ R. 

16. Derive the equations of contact for a unit sphere in rolling contact 
with a sphere of radius p. 

17. Kinematics of planar contact 

The kinematics of contact for two planar objects can be obtained by 
restricting equation (5.28) to the plane. Let g a f = (p, R ) £ SE(2) 
be the relative configuration of the objects. 

(a) Let s 0 £ M be the point of contact on the object and Sf £ R be 
the point of contact on the finger. Assume that the surface is 
parameterized by arc length, so that || = 1 where Ci : R —> 

R 2 is a coordinate chart. Show that the contact coordinates 
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r) = (sf, s 0 ) evolve according to 

(f)r T ■ 8r T ■ \ 

s f = (K 0 + K f )~ Rn f + Kogf o {Rc f +p)j 

s 0 = (K 0 + K f )- 1 l--^ Rn f + K f ^ (Rc f +p) j 

(b) Derive the equations of contact for a planar, elliptical finger 
rolling against a flat object. 

18. Consider the coordinated lifting problem shown below. Derive the 
constraints between the beam velocity and the robot joint velocities. 
Assume that each finger firmly grips the end of the beam and that 
the forces are transmitted to the beam at the center of the gripper. 



19. Rederive the contact kinematics when the finger and object surfaces 
are given as the level sets of functions hf : ffi 3 —> R and h 0 : R 3 —» R 
(for example, a sphere of radius p satisfies hf(x) = x\+x\+x1—p 2 = 
0). Test your solution using an ellipsoid rolling on a plane. 
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Chapter 6 

Hand Dynamics and 
Control 


In this chapter, we study the dynamics and control of a set of robots 
performing a coordinated task. Our primary example will be that of a 
multifingered robot hand manipulating an object, but the formalism is 
considerably broader. It allows a unified treatment of dynamics and con¬ 
trol of robot systems subject to a set of velocity constraints, generalizing 
the treatment given in Chapter 4. 


1 Lagrange’s Equations with Constraints 

For an open-chain manipulator, the equations of motion can be derived 
using Lagrange’s equations or other similar methods. This involves find¬ 
ing a set of generalized coordinates which completely and minimally pa¬ 
rameterize the configuration space of the system, and then writing the 
dynamics in terms of these coordinates and the corresponding generalized 
forces. 

For a multifingered robot hand, the configuration of the system de¬ 
pends on the joint angles for the fingers as well as the position and ori¬ 
entation of the object. These quantities are not independent, however, 
since their velocities are related by the grasping constraint. Thus, we 
cannot apply Lagrange’s equations directly. Finding a set of generalized 
coordinates in such examples is non-trivial and, in some cases, impossible. 

To overcome this difficulty, we rederive the equations of motion for a 
mechanical system in the presence of constraints. That is, rather than 
attempting to eliminate the constraints by an appropriate choice of coor¬ 
dinates, we seek to incorporate the constraints directly into the equations 
of motion. The remainder of this section contains a sketch of this deriva¬ 
tion. A more complete derivation can be found in Rosenberg [99] or 
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Pars [89]. 

For simplicity, we assume throughout this section that the configura¬ 
tion space Q is an open subset of R n with coordinates q = (qi ,..., q n ). 
More general configuration spaces can be handled by an appropriate 
choice of local coordinates. 

1.1 Pfaffian constraints 

A constraint on a mechanical system restricts the motion of the system by 
limiting the set of paths which the system can follow. A simple example 
is the case of two particles attached by an inextensible, massless rod. 
The configuration of each particle is described as a point pi el 3 , but all 
trajectories of the particles must satisfy the algebraic constraint 

\\pi-p 2 \\ 2 = L 2 , (6.1) 

where L is the length of the rod. The constraint acts through the ap¬ 
plication of constraint forces , which modify the motion of the system to 
insure that the constraint is always satisfied. In the case of the two inter¬ 
connected particles, the constraint force corresponds to the tension in the 
rod, which transmits forces applied on one particle to the other particle, 
and causes the distance between the particles to remain fixed. 

The constraint in equation (6.1) is an example of a holonomic con¬ 
straint. More generally, a constraint is said to be holonomic if it restricts 
the motion of the system to a smooth hypersurface in the (unconstrained) 
configuration space Q. Holonomic constraints can be represented locally 
as algebraic constraints on the configuration space, 

hi{q) = 0, i = l,...,k. (6.2) 

Each hi is a mapping from Q to R which restricts the motion of the 
system. We assume that the constraints are linearly independent and 
hence the matrix 


- dh i 

8 hi 

dqi 

dq n 

dh k 

dh k 

L dqi 

dqn 


is full row rank. (In the classical mechanics literature, constraints of 
the form in equation (6.2) are sometimes referred to as scleronomic con¬ 
straints. Holonomic and scleronomic come from Greek and mean re¬ 
spectively “all together lawful” and “rigid” respectively. Time-varying 
constraints on q are called rheonomic, for “flowing.” We will not use the 
terms scleronomic and rheonomic in this book, only the term holonomic.) 

Since holonomic constraints define a smooth hypersurface in the con¬ 
figuration space, it is possible to “eliminate” the constraints by choosing 
a set of coordinates for this surface. These new coordinates parameterize 
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all allowable motions of the system and are not subject to any further 
constraints. In fact, this is precisely the technique which we use when 
writing down the motion of a rigid body in terms of the position and 
orientation of a single coordinate frame rather than the (constrained) 
motion of the individual points of the rigid body. 

The constraint forces for a set of holonomic constraints of the form in 
equation (6.2) are linear combinations of the gradients of the constraint 
functions hi : Q —* R. Letting h : Q —> R fc represent the vector-valued 
constraint function, we can thus write the constraint force as 


where A £ is the vector of relative magnitudes of the constraint forces. 
These constraint forces can be viewed as acting normal to the constraint 
surface, with the magnitude of the forces chosen to insure that the system 
remains on the constraint surface defined by equation (6.2). Note that no 
work is done by the constraints when the system is moved along feasible 
trajectories since 

A fundamentally different type of constraint occurs in the context of 
multifingered grasping, where the allowable motions of the system are 
restricted by the velocity constraint 

J h (9,x 0 )6 = G T (0,x o )Vp O . 

More generally, for a system with configuration space Q, we consider 
velocity constraints of the form 


A(q)q = 0, 

where A(q) £ R fexn represents a set of k velocity constraints. A constraint 
of this form is called a Pfaffian constraint. We assume that the constraints 
are pointwise linearly independent and hence that A(q) is full row rank 
at q £ Q. For a multifingered hand, the matrix A has the form 

A(q) = [J h (q) -G T {q )] , 

where q = ( 0 , x) and x represents a choice of local coordinates for the 
object position and orientation. 

Since a Pfaffian constraint restricts the allowable velocities of the sys¬ 
tem but not necessarily the configurations, we cannot always represent 
it as an algebraic constraint on the configuration space. A Pfaffian con¬ 
straint is said to be integrable if there exists a vector-valued function 
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h : Q —> R fc such that 


A{q)q = 0 <t=> v^<i = 0. 

Thus, an integrable Pfafiian constraint is equivalent to a holonomic con¬ 
straint. It is important to note that we do not require that A = 
but only that they define the same subspace of the allowable velocities at 
every q £ Q. 

A Pfafiian constraint which is not integrable is an example of a non- 
holonomic constraint. Nonholonomic constraints of this type occur when 
the instantaneous velocities of the system are constrained to an n — k 
dimensional subspace, but the set of reachable configurations is not re¬ 
stricted to some n—k dimensional hypersurface in the configuration space. 
As we shall see in the next chapter, not all Pfafiian constraints are in¬ 
tegrable, and hence we must extend our derivation of the equations of 
motion to account for this case. 

Despite the possibility that a constraint may be nonholonomic, it is 
still possible to speak of the forces of constraint. They are the forces 
which are generated by a set of Pfaffian constraints so as to insure that 
the system does not move in the directions given by the rows of the 
constraint matrix A(q). The constraint forces at a configuration q £ Q 
have the form 

T = A T (q) A, 

where A £ R k is the vector of relative magnitudes of the constraint forces. 
If the constraint happens to be integrable, then this is identical to the 
holonomic case since A T (q) and ^ will have the same range space. 

The constraint forces for a set of Pfaffian constraints prevent motion 
of the system in directions which would violate the constraints. In order 
to include these forces in the dynamics, we must add one additional 
assumption about the nature of the constraints. Namely, we assume that 
the forces which are generated by the constraints do no work on the 
system, and hence conserve energy. This assumption is often referred to 
as dAlembert’s principle. 

The assumption that the constraints do no work is easy to justify in 
many situations. In particular, if a system is subject to rolling, either 
about a point or along a surface, then the frictional forces due to this 
rolling are very small and can usually be ignored. However, if some 
sliding occurs in addition to rolling, the work done depends upon the 
magnitude of the normal force, and the constraints cease to be workless. 
In cases such as these, friction and other nonconservative forces can be 
incorporated by ignoring them initially, and then adding them as external 
forces once the dynamics have been derived. 
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1.2 Lagrange multipliers 

We can now proceed to derive the equations of motion for a mechanical 
system with configuration q G I™ subject to a set of Pfaffian constraints. 
Let L(q , q) represent the Lagrangian for the unconstrained system and 
let the constraints have the form 


A{q)q = 0 A{q) G R fcxn 


(6.3) 


We assume that constraints are everywhere smooth and linearly indepen¬ 
dent and that the forces of constraint do no work on the system. 

The equations of motion are formed by considering the constraint 
forces as an additional force which affects the motion of the system. 
Hence, the dynamics can be written in vector form as 


dt dq 


dL 

dq 


+ A T (q) A-T = 0, 


(6.4) 


where the columns of A T form a non-normalized basis for the constraint 
forces and A G R fc gives the relative magnitudes of the forces of constraint. 
As before, T represents nonconservative and externally applied forces. 

The scalars A*,..., A*, are called Lagrange multipliers. They are de¬ 
termined by solving equations (6.3) and (6.4) for the n + k variables q 
and A, insuring that no motion occurs in the constrained directions, and 
hence equation (6.3) holds for all time. In general, each \ will be a 
function of q , q , and T, since the constraint forces vary with the con¬ 
figuration, velocity, and applied force. Solving for these multipliers and 
substituting them back into the equations of motion gives a description 
of the dynamics of the system. 

In the case that the Lagrangian has the form L(q, q) = A q T M(q)q — 
V(q) (kinetic minus potential), we can derive an explicit formula for the 
Lagrange multipliers. Using the notation from Chapter 4, the equations 
of motion can be written as 


M(q)q + C(q , q)q + N{q, q) + A T (q) A = F, (6.5) 

where F corresponds to the vector of external forces and N(q, q) includes 
nonconservative forces as well as potential forces. Differentiating the 
constraint equation (6.3) yields 

A(q)q + A(q)q = 0 

and, solving for q from equation (6.5), we obtain 

(AM~ 1 A t ) A = AM~\F — Cq — N) + Aq , 


where we suppress the dependence on q and q. The configuration depen¬ 
dent matrix AM~ 1 A T is full rank if the constraints are independent, and 


269 


mg 


Figure 6.1: Idealized planar pendulum. 


hence the Lagrange multipliers are 

A = (AM~ 1 A t )~ 1 -Cq-N)+ Aqj . (6.6) 

Using this equation, the Lagrange multipliers can be computed as a func¬ 
tion of the current state, q and g, and the applied forces, F. The equa¬ 
tions of motion are now given by equation (6.5) with A defined as in 
equation (6.6). 

Example 6.1. Dynamics of an idealized planar pendulum 

Consider an idealized pendulum, with its mass concentrated at the tip, as 
shown in Figure 6.1. The mass is a particle with two degrees of freedom; 
the pendulum mechanism acts as a constraint which restricts the motion 
to a single degree of freedom. Rather than parameterize the system in 
terms of the angular variable 0, we instead derive the equations using the 
method of Lagrange multipliers. 

The configuration of the system is given by q = (x,y) € R 2 . The 
constraint that the length of the pendulum remain fixed can be written 
as 

2,2 i2 

x + y = l . 

Differentiating this constraint and dividing by two, we obtain the Pfaffian 
constraint 

*1 = 0 . 
y 

A(q) 

The (unconstrained) Lagrangian is simply L(q, q) = | m{x 2 + y 2 ) — mgy. 

Substituting into equations (6.5), Lagrange’s equations for the con¬ 
strained system become 



m 0 


X 

0 TO 


y_ 


0 

mg 


x 

y 


A = 0. 
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The Lagrange multiplier A is determined from equation (6.6), which gives 


A = (. AM~ l A r )" 1 ( AM~ 1 {Q -Cq-N)- Aqj 

fTl , . o . 9 \ / .9 .9 \ 

= ^ 2 ~^y 2 (-9V-X -y ) = --p(gy + x +y), 
and hence the equations of motion are 


m 

O' 


X 

+ 

' 0 ' 

1 

0 

m 


y 

mg 

~ P 


(mgy + m(x 2 + y 2 )) = 0. 


Note that this is a second-order differential equation in two variables, x 
and y, even though the actual system only has one degree of freedom. 
Thus, we have increased the number of variables required to represent the 
motion of the system. On the other hand, we have an explicit measure 
of the tension in the rod supporting the mass, namely 


Tension = 



mq to .. 9 .9. 

-j-V+jiar + y 2 ). 


1.3 Lagrange-d’Alembert formulation 

It is convenient and useful to rederive the equations of motion without 
explicitly solving for the instantaneous constraint forces present in the 
system. In essence, this proceeds by projecting the motion of the system 
onto the feasible directions and ignoring the forces in the constrained 
directions. In doing so, we will be able to get a more concise description 
of the dynamics which is in a form well suited for closed-loop control. 

At a given configuration q € M n , the instantaneous set of directions 
in which the system is allowed to move is given by the null space of 
the constraint matrix, A{q). We adopt the classical notation and call a 
vector Sq £ 1™ which satisfies A{q)5q = 0 a virtual displacement. If F 
is a generalized force applied to the system, then we call 5W = F ■ Sq 
the virtual work due to a force F acting along a virtual displacement 6q. 
D’Alembert’s principle states that the forces of constraint do no virtual 
work. Hence, 


(A T (q) A) • Sq = 0 for A(q)Sq = 0. 

It is important to keep in mind that Sq is not the same as q. The general¬ 
ized velocity q satisfies both the velocity constraints and the equations of 
motion. The virtual displacement only satisfies the constraints. Hence, 
d’Alembert’s principle asserts that constraint forces do no work for any 
instantaneous motion which satisfies the constraints, not just for the mo¬ 
tion which the system actually follows. 
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To eliminate the constraint forces from equation (6.4), we project the 
equations of motion onto the linear subspace generated by the null space 
of A(q). Since (A T A) ■ Sq = 0, equation (6.4) becomes 


(±dL 

dq 


dL 

dq 


Sq = 0, 


(6.7) 


where Sq £ R n satisfies 

A(q)Sq = 0. (6-8) 

We call equations (6.7) and (6.8) the Lagrange-dAlembert equations. 
Note that in the case where there are no constraints on the system, Sq is 
free and equation (6.7) reduces to the usual form of Lagrange’s equations. 

To get a more explicit description of the dynamics, we assume that 
A(q) has the form 

A(q)=[A 1 {q) A 2 {q)\, 

where A 2 (q) £ R fexfc is invertible. This can always be achieved locally by 
reordering the configuration space variables. We now relabel the config¬ 
uration as q = (qi,q 2 ) £ R" _fc x R fe so that 

A(q)-Sq = 0 Sq 2 = 1 (q)A 1 (q)Sq 1 , 


where 5q\ is free (unconstrained). Making use of this same notation in 
the Lagrange-d’Alembert equations yields 


/_d dL 
\dt dq 


dL 

dq 



■ Sq 


= (±9L_dL_ T \ , 

V dt d(ji dq 1 1 ) qi \dtdq 2 dq 2 

= i_T 

\dtdqi dqi qi \dtdq 2 dq 2 

and since Sqi is free, the equations of motion become 


■Sq 2 


■(-a 2 1 a 1 ) 


Sqi, 


( d dL 
\ dt dqi 



A T iAi T 


/ d dL 
V dt dq 2 



(6.9) 


Equation (6.9) is a second-order differential equation in terms of q = 
( qi,q 2 )• We can further simplify this equation using the constraint q 2 = 
—Ai^ 1 A\q\ to eliminate q 2 and q 2 . The evolution of q 2 can be retrieved 
by reapplication of the constraint equations. This procedure is illustrated 
in the next example. 

Example 6.2. Dynamics of a rolling disk 

Consider the example of a disk rolling on the plane, as shown in Fig¬ 
ure 6.2. The configuration of the disk is given by the xy position in the 
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Figure 6.2: Disk rolling on a plane. 


plane, the heading angle 9 , and the orientation of the disk with respect 
to the vertical, qf>. We write q = {x, y, 9 , </>) and let p denote the radius of 
the disk. We take as inputs the driving torque on the wheel, tq, and the 
steering torque (about the vertical axis), Tq,. 

We make the assumption that the disk rolls without slipping, just as 
in the case of a fingertip rolling on an object. This condition can be 
written as a set of velocity constraints 


x — p cos 8(j) = 0 
y — p sin 9<j> = 0 


or 


A(q)q 


10 0 
0 1 0 


—p cos 6 
—psm9 


5 = 0. 


( 6 . 10 ) 


These constraints require that the disk roll in the direction in which it 
is heading and that the velocity of the disk match the rate at which it is 
rolling on the plane. The constraints are everywhere linearly independent. 

The Lagrangian for this system is the kinetic energy associated with 
the disk, ignoring the constraints. Let m be the mass of the disk, I\ its 
inertia about the horizontal (rolling) axis, and I 2 its inertia about the 
vertical axis. The Lagrangian is 

L{q, Q) = + V 2 ) + 


We can now proceed to derive the equations of motion for the system. 
Let Sq = ( 6x,5y,59,5(j )) represent a virtual displacement of the system. 
The Lagrange-d’Alembert equations are given by 



• Sq = 0 


where 


1 0 0 
0 10 


—pcos9 
—p sin 9 


Sq = 0. 


From the form of the constraints, we can solve for 5x and Sy to obtain 


6x = p cos 95(f) 
Sy = p sin 9S<\>. 


( 6 . 11 ) 
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The equations of motion can now be rewritten as 


0 0 


X 

mp cos 6 mp sin 6 


y_ 


Too 0 

0 T 2 



and since 69 and 6(f> are free, the dynamics become 


66 
64 J 


= 0 , 


0 0 


X 


Too 0 


9 


re 

mp cos 6 mp sin 6 


y. 

+ 

0 J 2. 


A. 


7<t>. 


( 6 . 12 ) 


We can further simplify the equations by reusing the constraints to 
eliminate x, y and x, y. Differentiating the constraints, we have 


x = p cos 6(f) — p sin 99(j) 
y = p sin 6<j> + p cos ddcj. i>, 


and substituting into equation (6.12) gives 

Too 0 

0 T 2 + mp 2 



6 


re 


A 


7<t>_ 


(6.13) 


(6.14) 


which describes the motion of the system as a second-order differential 
equation in 6 and <fr. Note that for this system the equations of motion 
for 6 and <fi do not depend on x and y. but in general this is not the case. 

The motion of the x and y positions of the disk can be retrieved from 
the first-order differential equations 


x = p cos 6<j> 
y = p sin 94>. 

Thus, given the trajectory of 6 and <j>, we can determine the trajectory of 
the disk as it rolls along the plane. The splitting of the equations of mo¬ 
tion into a set of second-order equations in a reduced set of variables plus 
a set of first-order equations representing the constraints is characteristic 
of nonholonomic systems. 


1.4 The nature of nonholonomic constraints 

The machinery that we have developed in this section allows us to cal¬ 
culate the dynamics of a mechanical system subject to a set of Pfaffian 
constraints without trying to integrate the constraints and find a set of 
generalized coordinates which completely (and minimally) parameterize 
the configuration of the system. In the case that the constraints are inte¬ 
grate, these equations are identical to those obtained by finding a set of 
appropriate generalized coordinates. When the constraints are noninte- 
grable, it is very important to incorporate the constraints in the proper 
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way. In this section, we try to shed some light on how nonintegrable 
constraints affect the mechanics of the system and indicate what can go 
wrong if one is not careful. 

A common mistake when deriving the equations of motion for a me¬ 
chanical system with nonholonomic constraints is to substitute the con¬ 
straints into the Lagrangian and then apply Lagrange’s equations. This 
would seem to eliminate the dependent variables and minimize compu¬ 
tations. As we shall see in a moment, however, this gives the wrong 
equations of motion for the system. We have been very careful in this 
section to always compute the unconstrained Lagrangian, substitute into 
the Lagrange-d’Alembert equations, and then reapply the constraints to 
eliminate the dependent variables. 

To see what happens when the constraints are used at the wrong time, 
consider a three-dimensional, unforced Pfafhan system with configuration 
q = (r, s) G R 2 x R, constraint 

s + a T (r)r = 0 a(r) £ R 2 , 

and Lagrangian L(r,f,s). For simplicity, we assume that both the La¬ 
grangian and the constraints do not depend on the variable s. Define the 
constrained Lagrangian as 

L c (r,r) = L(r,r,-a T (r)r). 


The constrained Lagrangian is the kinetic minus potential energy of the 
system, but evaluated with the constraints taken into account. 

Suppose we substitute the constrained Lagrangian into the Lagrange- 
d’Alembert equations. Since there is no dependence on either s or s, 
these equations yield 


d dL c dL c 
dt dri dri 


i = l,2. 


Expanding the equations using the definition of L c , we obtain 


dL 


dL 


dL dL 


da ,• 


It ( (r) Til) - ( ^7 - Til E ^ ) = 0 


dri ds 


dri 


^dii ds 

and, rearranging terms, we can write this equation as 
- ai(r) 


d dL 
dt dri 


dL 

dri 


d dL 
dt ds 


dL 

ds 


dL 

d.s 


dai 


y ^ 

^ dr t 
o 


cii(r) - > ^r. 


Notice that the left-hand side of this equation is exactly the Lag/&J*'^i 
d’Alembert equation for the system. The right-hand side of the equation 
represents spurious terms that arise from substituting the constraints 
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into the Lagrangian. This shows that, in general, substituting a set of 
Pfafhan constraints into the Lagrangian and then applying the Lagrange- 
d’Alembert equations gives the wrong equations. 

Suppose now that the constraint is actually holonomic. For simplicity, 
we assume that the constraint is explicitly integrable, so that there exists 
a function h{r ) such that 

dh 

».H = aV 

In this case, the right-hand side of equation (6.15) is 

— (a-(r) - V —ri) = — (Y ^ h f - V °’ 2h i 
ds 1 g r . 1 Qg l dndrj 3 Qr-dn 


which is identically zero since mixed partials commute. Hence, if the 
constraints happen to be holonomic, then substituting the constraints 
into the Lagrangian will give the correct equations of motion. 


2 Robot Hand Dynamics 

Using the results from the previous section, we now derive the equations 
of motion for a system of robots performing a coordinated task. We begin 
with a multifingered hand manipulating an object and then show how the 
same formalism can be applied to other problems. 

2.1 Derivation and properties 

The overall dynamics for a multifingered hand grasping an object are 
obtained by combining the dynamics of the fingers and the object via 
the grasp constraint. Adopting the notation of the previous chapters, the 
finger dynamics have the form 

M f (9)9 + C f (9,9)9 + N f (9 , 9) = t, (6.16) 

where 9 = (9f lt ... ,9 f k ) £ K” is the vector of joint angles for all of the fin¬ 
gers in the hand and r £ 1" is the corresponding vector of joint torques. 
Mf , Cf , and Nf are formed by appropriately stacking the quantities 
obtained for the individual fingers: 


'M fl 

0 


Cf 1 

0 ' 


'N h 



Cf = 



Nf = 


0 

M fk _ 


_ 0 

Cfk. 


N fk _ 


The object dynamics are given by the Newton-Euler equations, de¬ 
rived in Chapter 4. These equations have the form 


ml 

O' 


' v b ' 


ui b x mv b 

0 

1 


w 6 

+ 

0 J b x lu b 
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where V b = ( v b ,uj b ) is the body velocity of the object, F b is the body 
wrench, m is the mass, and I is the moment of inertia matrix, all with 
respect to a frame attached at the center of mass. We write x Q = (p, R) 
for the configuration of the object. If the object is subject to gravity 
alone, the dynamics can be written as 


ml 

O' 

T* rf) 

0 

1 

V + 


mco 

0 


0 

\(Q b J-lQ b ) 


V b + 


R T (mg) 
0 


= 0, (6.17) 


where g is a vector in the direction of gravity, usually taken to be g = 

( 0 , 0 , 1 ). 

In order to apply the Lagrange-d’Alembert equations, it is necessary 
to rewrite the Newton-Euler equations in local coordinates. Letting x £ 
K 6 be a local parameterization for x a £ SE(3), the object dynamics can 
be written as 

M 0 (x)x + C 0 (x, x)x + N 0 (x , x) = 0, 

which has the same form as the robot dynamics in equation (6.16). It 
can be shown that M 0 (x) > 0 and M a — 2 C 0 is skew-symmetric (see 
Exercise 4). 

The fingers and object are connected by the grasp constraints 

Jh(0, x)6 = G T (9 , x)x , (6.18) 


written here in local coordinates. We make three assumptions about the 
structure of the grasp: 

1. The grasp is force-closure and manipulable. Recall that this is 
equivalent to the conditions 

G(FC) = K p and R(G t ) C H(J h ) 
for all possible configurations. 

2. The hand Jacobian is invertible. This condition insures that there 
are no redundant motions of the fingers and hence the hand has 
exactly the number of degrees of freedom required to complete the 
task. 

3. The contact forces remain in the friction cone at all times. This 
condition is necessary in order to insure that the grasp constraints 
in equation (6.18) hold at all times. It is the task of the control law 
to insure that this condition holds, since otherwise the object can 
slip or drop from the grasp. 

The first two assumptions can be relaxed; we discuss those cases in the 
next section. 
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The Lagrangian for the composite system is given by 

L = i 9 T M f 9 + * x t M 0 x - V f (9) - V a (x ), (6.19) 

where V/ and V 0 are the potential energy terms due to gravity. Let 
q = ( 9 , x) represent the overall configuration of the system. 

We can now apply the results of the previous section to derive the 
equations of motion. The velocity constraint in equation (6.18) gener¬ 
ates a constraint on the virtual displacements 59 and 5x, namely 59 = 
J^ 1 G t 5x. Making use of this relationship, we can rewrite the Lagrange- 
d’Alembert equations as 


(±dL 

\(ft dq 


dL 

dq 



■ 5q 


d dL dL 
dt dO 

d dL dL 

dt dx dx 



d dL dL 
dt QO d9 
j—T f d dL 


r) • (J^G T Sx) + ( 


d dL 
dt dx 


dL 

dx 


5x 


= gj;‘ 


dt QO d9 


■ 5x - 


±dL_dL\ 

dt dx dx X ' 


where we have used the properties of the inner product in the final step 
to isolate 5x. Since 5x is free, 


(±dL_dL\ T (±dL_dL\_ T 

\dtdx dx) + h \dtd9 de)~ h 


( 6 . 20 ) 


Equation (6.20) together with the velocity constraints given in equa¬ 
tion (6.18) describe the system completely. Note that equation (6.20) 
is a second-order vector equation with n — m rows and equation (6.18) is 
a first-order vector equation with m rows. 

Since the Lagrangian for the system splits into two separate parts, 
the two terms on the left-hand side of equation (6.20) simplify into the 
object dynamics and the finger dynamics, respectively. Using this fact 
and eliminating 9 and 9 via the constraints, the equations of motion 
become 

M{q)x + C(q,q)x + N(q,q) = F, (6.21) 

where q = (9 , x) and 

M = M 0 + GJ- T M f J~ 1 G T 

( 6 . 22 ) 

N = N 0 + GJ~ T N f 
F = GJ^ t t. 


C = C 0 + GJ^ T (c f J^G T + M f j t (J,7 1 G t ) 
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These equations have the same form as the equations for a single open- 
chain manipulator. M is called the the effective mass of the object, 
C the effective Coriolis matrix, and N the effective gravitational and 
nonconservative forces. These quantities include the dynamics of the 
fingers; however, the details of the finger kinematics and dynamics are 
effectively hidden in M, C , N. The following lemma verifies that these 
equations also satisfy the same structural properties as the unconstrained 
case. 

Lemma 6.1. Structural properties of the equations of motion 

Equation (6.21) satisfies the following properties: 

1. M(q) is symmetric and positive definite. 

2. M ( q ) — 2 C is a skew-symmetric matrix. 

Proof. Since the grasp is assumed to be force-closure and manipulable 
and Jh is assumed injective, property 1 follows from its definition. To 
show property 2, 

lif-2C=(M 0 - 2 C 0 ) + GJ~ T {M f - 2 C f )J~ 1 G T 

+ ± ( GJ~ T ) M f J-'G T - GJ~ T M f j t {J~'G t ) 

The first line is the sum of skew-symmetric pieces. Taking transposes 
and using symmetry of Mf inverts the sign of the last line, and hence it 
too is skew-symmetric. □ 

Equation (6.21) was derived in terms of a local parameterization of 
SE( 3). This was necessary since the Lagrange-d’Alembert equations only 
make sense for q £ R”. Since SE( 3) is not locally Euclidean, we cannot 
apply the Lagrange-d’Alembert equations directly in terms of x 0 £ SE( 3). 
However, having derived the equations of motion in local coordinates, we 
can now revert to SE( 3) and write the dynamics as 

M(q)V 0 b + C(q , q)V b + N(q , q) = F b , (6.23) 

where M 1 C , and N are described precisely as before except using the 
global versions of the inertial parameters. The proof of this fact is left as 
an exercise. We make use of the local parameterization of the system for 
the remainder of this section since it simplifies the exposition. 

2.2 Internal forces 

In Chapter 5, we saw that if a grasp is force-closure, then there exist 
contact forces which produce no net wrench on the object. These forces 
are called internal forces. Their presence in the dynamics is seen in the 
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forcing term F = GJ^ t t, which maps joint torques into object forces. If 
J^ T t £ A/"(G), then no net wrench is generated. 

However, even if J^ T t has no component in Af(G), internal forces may 
still be present in the system. These internal forces can arise due to the 
constraint forces which the Lagrange-d’Alembert equations eliminated. 
Defining q = ( 9,x ), the full equations of motion can be written as 


d dL 
dt dq 


dL 

dq 


+ A T (q)\-r = 0, 


where A £ R m is the vector of Lagrange multipliers and the columns of 
A T (q) correspond to the constraint directions. For a multifingered grasp 
we can choose 

A(q) = [-J h (0,x ) G T (8,x)] 
and the full equations of motion have the form 


-M f 

0 ' 

6 

_L 

c f 

0 ' 

'6 


N/ 

0 

M 0 _ 

X 

i 

0 

G 0 

X 

i 

N o_ 




(6.24) 


We see immediately from equation (6.24) that the Lagrange multipliers 
A £ R m can be interpreted as contact forces. The net constraint forces 
act as external forces applied at the tip of the fingers and at the contact 
points on the object. If A has a component in the null space of G, then 
internal forces will be present. 

To solve for the instantaneous forces during motion, we solve for the 
Lagrange multipliers using the results of Section 1.2. Letting M, G, and 
N represent the block matrices in equation (6.24), we have 


A = (AM~ 1 A t )- 1 




Cq-N 



(6.25) 


We can compute the internal forces as a function of the current configura¬ 
tion, velocity, and applied torques by projecting A onto the null space of 
G. The computation of A and its projection is evidently extremely messy. 
In practice, the internal forces generated by non-quasistatic motion are 
either ignored or measured directly (via force sensors at the contacts). 

An alternative way to find the constraint forces is to solve for them 
in terms of the joint accelerations. If Jh is invertible, the contact forces 
satisfy 

A = Ju T (T-M f B-C f B-N f ). 

From this equation it is clear that if the system is in motion, then internal 
forces may exist even if J^ T t has no component in the null space of 
G: the dynamic terms may generate internal forces. Notice also that 
A = f c := J^ T t only when the system is at rest. 
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Figure 6.3: Coordinated lifting. 


All of the analysis presented here relies on the assumption that the 
fundamental grasping constraint holds at all times. Even though we are 
solving for the contact forces, it is important to remember that the con¬ 
tact forces are generated by the constraints. If the contact forces fall 
outside of the friction cone, then the velocity constraints on the system 
no longer hold and the equations of motion are no longer given by equa¬ 
tion (6.24). It is one of the tasks of control system to keep the contact 
forces inside the friction cone so that the models we use here remain valid. 

2.3 Other robot systems 

Although the analysis so far has been motivated by grasping, the basic 
formulation holds for any robot system subject to constraints of the form 

J(q)9 = G T (q)x 

with q = (0, x). Any such system will automatically have dynamics with 
the same form and structure as those of an open-chain manipulator. In 
this section, we describe several such examples. 

Coordinated lifting 

Consider a robotic system which consists of several individual robots 
lifting a single object, such as the example shown in Figure 6.3. Assume 
that each robot firmly grasps the object and can exert arbitrary wrenches 
without losing contact. 

To derive the kinematics of this system we can treat each robot as a 
single finger. The contact model for a robot firmly grasping an object 
is given by B Ci = I G M. pxp and FC = R p . This is just the mathe¬ 
matical statement that the robot can exert arbitrary forces and torques. 
Substituting these relations into the grasp constraint and identifying the 
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Figure 6.4: The Motoman K10MSB robot performing an arc welding 
task. (Photograph courtesy of Motoman) 


contact and tool frames, we have 



0 


[Ad* 1 , 1 

yoti 



e = 


0 

Ad s«* t k J t k t k 


Ad* 1 , 


-V- V- 

J G T 

The dynamics of the system follow exactly as in the grasping case. 


Workspace dynamics 

Many robot systems perform tasks which are most naturally described in 
workspace coordinates rather than joint space coordinates. For example, 
in the welding application depicted in Figure 6.4, a natural description of 
the task would be in terms of the position and orientation of the welding 
tip. Rather than solve an inverse kinematics problem to generate the 
corresponding path in joint space, it is possible to directly specify the 
dynamics in terms of the workspace coordinates (SE( 3) for this example). 

For simplicity, we work in local coordinates. Let g : Q —> M p represent 
the forward kinematics of the manipulator and J{6) = || the Jacobian. 
We assume that J is square and nonsingular. We take dynamics of the 


282 








object held by the robot (e.g., the arc welder) as 

M a (x)x + C Q {x, x)x + N a (x, x) = 0, 

where x £ R p represents the workspace coordinates and the usual struc¬ 
tural properties are satisfied by M 0 and C 0 . 

The kinematics of the mechanism is given by 

J{6)9 = x, 

which has the form of our canonical constraint with G = I £ R pxp . Thus, 
we can write the dynamics as 

M(q)x + C(q,q)x + N(q,q) = F, (6.26) 

where q = (9, x) and 

M = M a + J~ T M f J~ x 
C = C 0 + J~ T + 

N = N a + J~ T N f 

F = J~ t t 

(quantities with the subscript / refer to the robot dynamics). It follows 
immediately that M > 0 and M — 2 C is skew-symmetric. 

The dynamics given in equation (6.26) represent the equations of mo¬ 
tion relative to the workspace coordinates x £ R p . Thus, M(q) represents 
the inertia of the system as viewed from the object frame of reference. As 
in the grasping case, M(q) incorporates both the object inertia and the 
inertia of the robot (at its current configuration). If the robot approaches 
a singular configuration, the inertia matrix becomes unbounded. This is 
because large workspace forces produce small object accelerations, and 
hence the effective inertia appears very large. However, this singularity 
is strictly due to the parameterization of the dynamics in terms of the 
workspace coordinates. The dynamics of the mechanism in joint coordi¬ 
nates are never singular. 

There are several variations on this problem. The dynamics can be 
written in terms of x 0 £ SE( 3) by replacing x with x 0 , x with Vjf, and x 
with Vg (see the comments at the end of Section 2.1). We can also write 
the dynamics even if no object is present, by setting M a , C 0 , and N a to 
zero. This is useful if we simply wish to command the trajectory of a 
robot in end-effector coordinates. Finally, we can in certain cases relax 
the assumption that J(9) be square. This is discussed in more detail in 
Section 3. 

The primary difference between this class of examples and grasping 
is the lack of any “internal forces” (since G = I never has a null space). 
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Hybrid position/force dynamics 

Another common manipulation task is one which consists of moving the 
robot in certain directions while pushing in other directions. An elemen¬ 
tary example is writing on a chalkboard: the task specification consists 
of a desired motion in the plane of the chalkboard and a desired force 
against the chalkboard. A preliminary discussion of this topic is con¬ 
tained in Chapter 4. We now use the tools developed in this chapter to 
describe this situation more completely. 

To analyze the kinematics of this system, we assume that the end- 
effector is required to satisfy a holonomic constraint of the form 

h{0,x) = O, (6.27) 


where x £ R p parameterizes the allowable motions of the manipulator. 
For example, when writing on a chalkboard, x might specify the location 
of the chalk on the board as well as its orientation (in some suitable set of 
local coordinates). More generally, h(0,x) specifies a p-dimensional sur¬ 
face in the configuration space of the manipulator. The task description 
consists of motion along this surface and generalized forces against this 
surface. 

Equation (6.27) can be converted into the standard problem by dif¬ 
ferentiating with respect to time: 


dh • dh . 

J G T 


(6.28) 


As before, we assume that J is square and nonsingular, indicating that 
no internal motions are present and that the manipulator is not at a 
singular configuration relative to the task. Internal forces correspond to 
joint torques r such that GJ~ t t = 0. These are precisely the torques 
which generate forces against the constraint surface. 

To formulate the dynamics of the mechanism, we assume that the 
object held by the robot is accounted for in the robot dynamics, and 
hence 

M(q)x + C(q,q)x + N(q, q) = F, 

where 

M = GJ~ T MfJ~ 1 G T 

C = GJ~ t (c f J~ l G T + M f d df (J-^)) 

N = N 0 + GJ~ T N f 
F = GJ~ t t. 

The reason for combining the object dynamics with those of the robot 
is that x £ R p may not actually correspond to the configuration of an 
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object in many applications. For instance, in the chalkboard example, 
only two of the linear velocities are specified by x. If the chalkboard were 
curved instead of flat, correctly specifying the object dynamics in terms 
of x becomes much more involved. 

As with grasping, internal forces do not affect the equations of motion 
and hence they can be ignored if only the free motion is to be studied. 
If internal forces are to be controlled or regulated, they can be found by 
solving for the Lagrange multipliers. 

Several variations of this problem are possible. The use of local co¬ 
ordinates for motion which is constrained to a subgroup of SE( 3) can be 
relaxed by appropriate interpretation of velocities and accelerations. In 
addition, the specifications of the task need not be in the form of a holo- 
nomic constraint. For some problems, it may be more natural to specify 
the kinematics directly in terms of J and G. 


3 Redundant and Nonmanipulable Robot Sys¬ 
tems 

In order to perform a given task, a robot must have enough degrees of 
freedom to accomplish that task. In the analysis presented so far, we 
assumed that our robots had exactly the number of degrees of freedom 
required to complete the task. That is to say, we assumed that each 
finger could follow the object through any allowable trajectory, but that 
the fingers had only the number of degrees of freedom required by the 
contact type (i.e., one degree of freedom for a frictionless point contact, 
three for a point contact with friction, and four for a soft-finger contact). 
This assumption manifested itself in our requirement that Jh be an in¬ 
vertible square matrix when we derived the dynamics. We now relax that 
requirement and discuss its consequences. 

There are two situations in which Jh can fail to be square and invert¬ 
ible: 

1. The manipulator has too many degrees of freedom. In this case, 

Jh will have two or more columns which are linearly dependent, 
allowing internal motions which leave the contact locations fixed. 

2. The manipulator has too few degrees of freedom. If Jh is not full row 
rank, it is not possible for the fingers to follow arbitrary motions of 
the contact points. This potentially limits the motion of the object, 
though it is possible for this situation to occur even if the grasp is 
manipulable. 

These cases are not distinct; it is possible for a manipulator to have both 
internal motions and fail to be manipulable at the same time. In any case, 
we seek to cast the problem into the general framework developed in the 
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previous section by augmenting or decreasing the number of variables 
used to describe the task. 

As before, we note that the material contained herein applies not only 
to multifingered hands, but to many other constrained systems as well. 
A few of these variations are described in the exercises. 

3.1 Dynamics of redundant manipulators 

Unlike conventional robot manipulators, constrained robot manipulators 
do not need to have more than six degrees of freedom in order to be 
redundant. The constraints themselves can introduce kinematic redun¬ 
dancy into a system. For example, if we attach a six degree of freedom 
finger to an object using a soft-finger contact, we have introduced two 
redundant degrees of freedom: the finger is free to roll in either of two di¬ 
rections without affecting the position of the object. Thus, it is absolutely 
essential that we include redundant mechanisms in our formulation. 

It is interesting to note that there are actually two sources of redun¬ 
dancy introduced by our constraints: kinematic redundancy and actua¬ 
tor redundancy. Kinematic redundancy refers to motions of the fingers 
which do not affect the motion of the object. Actuator redundancy refers 
to forces applied by the fingers which do not affect the object motion, 
i.e., internal forces. The grasping constraint 

Jh(0,x)9 = G t (9,x)x 

contains all the information necessary to determine these redundancies. 
Namely, the null space of Jh describes the set of joint motions which do 
not affect the motion of the object and the null space of G is precisely the 
space of internal forces. Since we have already discussed internal forces, 
we restrict our discussion to kinematic redundancy. 

Consider first the kinematics of a single redundant manipulator, with 
no constraints. If we are willing to control the manipulator in joint space, 
the dynamics formulation presented above holds without modification. 
However, in order to perform a task specified in terms of the configu¬ 
ration of the end-effector, we must first choose a joint trajectory which 
accomplishes this task. Suppose instead that we wish to write our con¬ 
trollers directly in end-effector coordinates. We represent the kinematics 
as a function g : K" —> SE( 3). In this case, the manipulator Jacobian 
J{9) := Jst(9) £ R pxn is not square, so J -1 is not well defined and we 
cannot write 9 in terms of V s s t as we did previously. 

It is still possible to write the dynamics of redundant manipulators in 
a form consistent with that derived earlier. To do so, we define a matrix 
K(9) € r(" _ p ) X71 whose rows span the null space of J(9). As before, 
we assume that J(9) is full row rank and hence K(9) has constant rank 
n — p. The rows of K{9) are basis elements for the space of velocities 
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which cause no motion of the end-effector; we can thus define an internal 
motion , vn £ R" _p , using the equation 


X 


J' 

. VN . 


K 


By definition, J is invertible and it follows from our previous derivation 
that 


M(q) ? +C(q,q) * +N{q,q) = J T r, 

v N v n 


where M and C are obtained as in the nonredundant case, but replacing 
J with J and G with I: 


M = J~ T MJ~ X 



N = N 0 + J~ T N f . 


If we choose K such that its rows are orthonormal and also perpendicular 
to the rows of J, then J -1 = [j + where J + = J T (JJ T ) _1 is the 

least-squares right (pseudo-) inverse of J. 

Note that we have parameterized the internal motion of the system by 
a velocity and not a new variable y. We do this out of necessity: since we 
chose K only to span the null space of J, there may not exist a function 
h such that y = h(6) and jjg = K. A necessary and sufficient condition 
for such an h to exist is that each row of K satisfy = d ^ g ik . This 
is merely the statement that mixed partials of h must commute. A more 
thorough treatment of this point is given in Chapter 7, and is illustrated 
briefly in the next example. 

In general, it may not always be easy to choose K{9) such that it is 
the differential of some function. For this reason, we shall not generally 
assume that an explicit coordinatization of the internal motion manifold 
is available. Thus, in the same way as we were forced to use velocity re¬ 
lationships when modeling constraints, we also use velocity relationships 
for redundant manipulators. The Lagrange-d’Alembert formalism lets us 
treat this case without difficulty. 

Example 6.3. Three-link planar manipulator 

Consider a three-link planar manipulator with unit-length links, as shown 
in Figure 6.5. If we let (x,y) be the location of the end-effector, then 


x = cos 9i + cos 9 2 + cos 9 3 


y = sin 9\ + sin 9 2 + sin 9 3 
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Figure 6.5: Three-link planar manipulator with joint angles measured 
relative to the horizontal axis. 


where the link angles are all with respect to a fixed (inertial) axis. The 
Jacobian of the mapping 0 i—> (x, y) is 


m 


— sin 9\ — sin 62 — sin O 3 

cos 61 cos 0 2 cos 0 3 


There are many choices of K(0) to complete J{9). If we choose 

K{0) = [0 0 1 ] , 

then K = || with h(9) = 0 3 . This corresponds to choosing the angle of 
the end-effector to parametrize internal motions. This choice of K{9) is 
valid as long as 9\ ^ 8 2 (i.e., when the first two links are not aligned). If, 
on the other hand, we choose 


K(9) = [sin(0 2 - 0 3 ) - sin(0i - 63 ) sin(0i - 0 2 )] , 


which is valid as long as all three links are not aligned, then 


dK i(0) 
de 2 


cos(0 2 - 63) ¥= - cos(0i - 03) 


dK 2 { 6 ) 

d9 1 


Hence, there is no h such that = K and the velocity vn = K(6)9 is 
not the derivative of a variable y. 

We now derive the equations of motion for the system in terms of x, 
y, and Vn . Let M(9) be the inertia matrix for the manipulator in joint 
space with C(0,0), the corresponding Coriolis matrix. For brevity, we 
ignore the potential and nonconservative forces. The dynamics of the 
mechanism in end-effector coordinates are given by 



(6.29) 
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where 


-J(9) 

W)J 


— sin 9\ 

— sin 02 

— sin 0 3 

= 

COS 

cos 0 2 

cos 0 3 


_sin(0 2 - 03 ) 

sin (0 3 - 0 2 ) 

sin(0! - 0 2 )_ 


We now return to the general case and extend our treatment to include 
the full grasp constraints. Consider a force-closure and manipulable grasp 
with velocity constraints 

J h 0 = G t x , 

where M(Jh) ^ 0. As before, we augment the constraint by choosing 
a matrix Kh(0) whose rows span the null space of Jh(9)- The grasp 
constraint can now be written as 



0 = 


G T 

0 


x 

Vi V 


G T 


where Jh and G represent the augmented hand Jacobian and grasp ma¬ 
trix. This constraint has the same form as the standard (nonredundant) 
grasp constraint and Jh is now invertible by construction. Hence, we can 
write the dynamics as 

M(q) 

where M, (7, and N are 

M = GJ^ t MJ^G t 

(VG T )) - 

We now see that redundant manipulators can be incorporated into 
the same general framework as other robot systems. The necessity of 
augmenting the description of the system stems from our use of the task 
variables, parameterized by x , to specify the motion of the system. Since 
the mechanism is redundant, the x variables alone do not provide suffi¬ 
cient information to determine the motion of the system. Augmenting 
the task description with the variables vn gives a complete description 
of the motion of the system. 

One final comment is in order regarding the relationship between the 
joint torques and the object wrench for a redundant grasp. In Chapter 5, 
we derived the static relationships between the joint torques, the contact 
forces, and the object wrench. These relationships were used to determine 
how to push on an object, via the fingers, in order to resist applied forces. 


C = GJh T (CJ^G T + M± 


X 

v N 


C(q,q) 


X 

vn 


= GJ~ t t, 


(6.30) 
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In the redundant case, a bit of care must be taken in interpreting these 
results. 

Consider the general grasping situation described above. Reverting 
to twists, the kinematic constraints have the form 


'Jh 

6 = 

G T 

O' 


\v b ~\ 

v po 

K h 

0 

I 


y N _ 


where V po £ R p is the object’s body velocity and Vn £ R” p is the 
internal velocity. The associated quasistatic forces satisfy 


= [J\ 


) 


fc 

In 


F b po = Gf c 


(6.31) 


The forces /jv parameterize the forces which correspond to internal mo¬ 
tions vn. If these forces are chosen to be zero, then we retrieve the usual 
force relationships between joint torques and object wrenches. 

If the forces /jv are not chosen to be zero, then the manipulator will 
begin to exhibit internal motions. This motion can cause accelerations 
of the manipulator joints and we can no longer use equation (6.31) to 
represent the force relationships in the system. Instead, we must consider 
the full equations of motion as given in equation (6.30). In particular, the 
internal motions of the system may generate constraint forces and hence 
the relationships in equation (6.31) are no longer correct. This situation 
does not occur in nonredundant systems since if we keep the end-effector 
fixed, then all joint angles also remain fixed and hence no dynamic terms 
are present. 


3.2 Nonmanipulable grasps 

We now consider the situation in which the hand Jacobian is not full row 
rank. In this case, there are some motions of the individual contacts which 
cannot be tracked by the fingers. We assume that the hand Jacobian is 
full column rank. If not, the methods of the preceding subsection can be 
used to augment the grasp with appropriate internal velocities. 

In most situations, if the hand Jacobian is not full row rank, then the 
grasp fails to be manipulable. However, in certain special situations, it is 
possible that a multifingered grasp is both manipulable and nonredundant 
but Jh is not square. This can occur when the structure of the grasp is 
such that Jh is bijective onto the range of G T but is not surjective as a 
map from R n —> R m . This situation almost never occurs in practice, and 
hence we concentrate here only on the case where Jh is nonmanipulable. 

To treat the nonmanipulable case, we must restrict the motions of 
the object to those which can be accommodated by the fingers. This 
restriction is enforced by structural forces within the hand, which resist 
motion of the system in directions in which the fingers are unable to move. 
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As usual, our formulation assumes that the contacts are maintained and 
hence the contact forces must remain inside the friction cone at all times. 
It is the responsibility of the control law to insure that this condition 
holds at all times. 

Consider a nonmanipulable grasp with grasp constraint 

J h 9 = G t x. 


Let W ( 9 , x) represent the space of allowable object velocities, 
W{9,x) ={ieP:3kr with J h 9 = G T x}. 


We assume that W(9,x) has constant dimension l > 0 and that W varies 
smoothly as a function of its arguments. Choosing a matrix H(9,x) £ 
M pxi whose columns span W{9,x), we can write the grasp constraints as 


J h 9 = G t Hw 
x = Hw , 


(6.32) 


where w GM 1 represents the object velocity in terms of the basis formed 
by the columns of H. 

To formulate the equations of motion, we write the dynamics in terms 
of the velocities w £ R z . By construction, Jh is surjective onto the range 
of G t := G T H and hence we can solve for 9 given w. However, Jh is not 
necessarily square so we must use the pseudo-inverse J£ = (Jj Jh )- 1 Jh 
in place of J^ 1 • The resulting dynamics are given by 


M(q)w + C(q,q)w + N(q,q) = F, (6.33) 


where q = ( 9 , x), 


M = M 0 + GJ+ T M f J+G T 

C = C 0 + GJ+ T (c f J+G T + M f (] df 

N = N 0 + GJ+ T N f 
F = GJ+ t t , 


(4 + G T ) 


and J^ T is the transpose of j£. The second-order dynamics in equa¬ 
tion (6.33) combined with the first-order constraints in equation (6.32) 
give a complete description of the motion of the system. 


3.3 Example: Two-fingered SCARA grasp 

To illustrate the results of this section, we consider the dynamics of two 
SCARA fingers grasping a box, as shown in Figure 6.6. Each finger is 
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Figure 6.6: Two-fingered grasp using SCARA robots. 


modeled as a soft-finger finger contact. The fundamental grasp constraint 
for the system has the general form 


8 

Jhl 0 

II 

GI 


U Jh2 




00 


00 


Although Jh{9) £ R 8x8 is a square matrix, it is not invertible. It was 
shown in Section 5.3 of Chapter 5 that the grasp is not manipulable and 
also contains internal motions. The lack of manipulability comes from the 
fact that rotations about the line connecting the contacts violate the soft- 
finger contact constraints. The internal motions correspond to motions 
of the first three joints of each finger which leave the xy positions of the 
fingertips fixed. 

To parameterize the internal motion of the system, we augment the 
system using the angles of the last joint of the fingers, as in Example 6.3. 
Letting y = (0 n + 0 12 + 0i 3 ,0 2 i + $22 + # 23 )) the contact constraints 
become 


10 


1 

0 ' 

Jh2 

0 = 

- Gl 

Gl i 

1 

0 0 


\v b 1 

v po 


1110 

0 0 0 0 

0 0 0 0 
1110 






0 

I 


L v \ 


8 8 


Note that for this example we were able to choose actual variables to 
parameterize the internal motions and not just velocities. 
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The addition of the internal variables does not alter the nonmanip- 
ulable nature of the grasp since Jh still does not span the range of G T . 
Because rotations of the object about the line connecting the contacts 
are the source of the difficulty, we eliminate these directions from the 
allowable velocities of the system. Choosing 


1 0 0 0 0 

0 0 

0 10 0 0 

0 0 

0 0 10 0 

0 0 

0 0 0 1 0 

0 0 

0 0 0 0 0 

0 0 

0 0 0 0 1 

0 0 

0 0 0 0 0 

1 0 

0 0 0 0 0 

0 1 


the resulting contact becomes 


//' 

0 

0 

I 


Jhl 

0 


' GjH' 

0 ' 


K1 

0 

Jhl 

0 = 

GlH' 

0 



Ai 

1<2 


0 

I 


[y \ 








8 * 4 7 


where V£ a = H'w' is in the set of allowable object velocities. A detailed 
calculation verifies that this constraint is manipulable and that no in¬ 
ternal motions exist. We can now solve for the dynamics of the system 
in terms of the workspace variables w = ( w',y ) £ R 7 , keeping in mind 
that the pseudo-inverse of Jh must be used since Jh is surjective onto the 
range of G T but not square. 


4 Kinematics and Statics of Tendon Actua¬ 
tion 

In many robot applications, it is difficult to control the torques on the 
joints directly, due to the size of the actuators required to exert reasonably 
sized forces. A more practical approach is to use a transmission network 
to carry forces from an actuator to the appropriate joint. Such a network 
typically consists of some combination of linkages, tendons, gears, and 
pulleys. 

In this section, we consider one of the more common transmission 
systems, a network of tendons. Tendons offer advantages in terms of 
weight and flexibility; however, they can complicate the kinematics of the 
mechanism. The basic problem which we study is to describe how forces 
applied at the end of a tendon relate to the joint torques applied to the 
mechanism. We also examine a second, somewhat less-common situation 
in which elastic tendons are driven by position-controlled actuators, such 
as a stepper motor. 
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Figure 6.7: A tendon-driven finger. 


4.1 Inelastic tendons 

Consider a finger which is actuated by a set of inelastic tendons, such 
as the one shown in Figure 6.7. Each tendon consists of an inextensible 
cable connected to a force generator, such as a DC motor. For simplicity, 
we assume that each tendon/actuator pair is connected either between 
the base of the hand and a link of the finger, or between two links of the 
finger. Interconnections between tendons are not allowed. We wish to 
describe how forces applied at the end of the tendons are related to the 
torques applied at the joints. 

Note that even though each tendon can be connected to only one 
link, pulling on a tendon may generate forces on many joints. This occurs 
because as we pull on a tendon, it exerts forces all along its length against 
whatever parts of the mechanism are holding it in place. This coupling 
is difficult to eliminate without awkward routing of the tendons. 

We model the routing of each tendon by an extension function , hi : 
Q —> R. The extension function measures the displacement of the end 
of the tendon as a function of the joint angles of the finger. For simple 
tendon networks composed of pulleys, such as those shown in Figure 6.7, 
the tendon extension is a linear function of the joint angles 


hi(0) — li A A ■ ■ ■ A ni n 0r t 


where h is the nominal extension (at 9 = 0) and is the radius of the 
pulley at the jth joint. The sign depends on whether the tendon path 
gets longer or shorter when the angle is changed in a positive sense. 

More complicated tendon geometries may involve nonlinear functions 
of the joint angles. For example, for the joint pictured in Figure 6 . 8 , the 
top tendon has an extension function of the form 


h\{0) = h A 2 \Ja 2 A b 2 cos 



-2b 9 > 0 , 
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Figure 6.8: Example of tendon routing with nonlinear extension function. 


while the bottom tendon satisfies 


h 2 (9)=l 2 + R9, 9> 0. 

When 9 < 0, these relations are reversed. 

Once the tendon extension functions have been computed, we can de¬ 
termine the relationships between the tendon forces and the joint torques 
by applying conservation of energy. Let e = h(9) £ R p represent the 
vector of tendon extensions for a system with p tendons and define the 
matrix P{9) £ R nxp as 

Then, 

e=^(0)0 = P T (0)0. 

Since the work done by the tendons must equal that done by the fingers 
(in the absence of friction or other losses), we can use conservation of 
energy to conclude 

T = P(0)f , 

where / £ R p is the vector of forces applied to the ends of the tendons. 
The matrix P{9) is called the coupling matrix and plays a role similar to 
that of the grasp map defined in Chapter 5. 

The kinematics of the tendon network can be combined with the dy¬ 
namics of the mechanism to yield 

M{9)9 + C{0,9)9 + N(9,9) = P(0)f. 

The structure of this equation relies on the assumption that the actuator 
and tendon dynamics can be ignored, and hence a force applied at the 
end of the tendon is immediately transmitted to the joints through the 
coupling matrix. 
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Figure 6.9: Planar tendon-driven finger. 


Example 6.4. Two-link tendon-driven Anger 

As an example, we consider the planar finger shown in Figure 6.9. It 
consists of two revolute joints driven by four tendons. The tendons are 
routed through sheaths attached to the sides of the links. 

The extension functions for the tendon network are calculated by 
adding the contribution from each joint. The two tendons attached to 
the first joint are routed across a pulley of radius R±, and hence 

fi-2 = I 2 ~~ R\@i 

h3 = I3 + R\6\. 

The tendons for the outer link have more complicated kinematics due to 
the routing through the tendon sheaths. Their extension functions are 

h 1 = l 1 + 2 \J a 2 + b 2 cos (tan -1 (7) + - 2 b- R 2 9 2 

\ 0 / 2 u\ > U. 

— I4. H - R\6\ -\- R2O2 

When 9i < 0, these relations are reversed. 

The coupling matrix for the finger is computed directly from the ex¬ 
tension functions. When 9\ > 0, 

p(0) — — T — -Va 2 +b 2 sin(tan _1 (|) + -R ± R x R 1 

U “ 89 ~ [ -R 2 0 0 R 2 ‘ 

Note that pulling on the tendons routed to the outer joints (tendons 1 
and 4) generates torques on the first joint as well as the second joint. 

4.2 Elastic tendons 

The preceding kinematic analysis can also be extended to elastic tendons. 
We assume that the tendons are completely free to slide along the fingers, 
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Figure 6.10: Planar finger with position-controlled elastic tendons. 


and hence we can lump all elasticity into a single spring element at the 
base of the tendon, as shown in Figure 6.10. We further assume that the 
tendon is massless and hence has no dynamics. In practice, this is a good 
approximation since tendon networks are usually much faster than the 
dynamics of the underlying robot. 

Since tendons are one-dimensional devices, the force relationships de¬ 
rived for inelastic tendons also hold in the case when the tendons are 
elastic. To see why this is so, consider the instantaneous effect of apply¬ 
ing a force to the end of a tendon. Assuming the tendon has negligible 
mass, the tendon will immediately stretch until the force due to the dis¬ 
placement of the tendon balances the applied force. However, in this 
case, the tendon will be applying exactly the same amount of force to the 
mechanism, and hence our previous analysis holds. 

When elastic tendons are used, it is also possible to control the position 
of the end of the tendon and use the elasticity of the tendon to convert 
this into a force. Let e, be the extension of the tendon as commanded 
by the actuator and let hi(0) be the extension of the tendon due to the 
mechanism. We assume that when 9 = 0 and ei = 0 the tendon is under 
zero tension. The net force applied to the tendons is given by 

fi = h{ei + hi{0 ) - hi(6)), 

where /c,; is the stiffness of the tendon. 

Letting K be the diagonal matrix of tendon stiffnesses, we have, for 
a completely elastic network of tendons with extension e, 

f = K(e + h(O)-h(0)) 
and the dynamics become 

M(0) + C(0,0)0 + N(0,0) + PK{h{0) - h( 0)) = PKe 

The function S(0) := PK(h(0) — h{ 0)) models the stiffness of the tendon 
network while Q := PK becomes the new coupling matrix between the 


297 











tendon extension and the equivalent joint torques. If the input positions 
are constant, then S(9) gives the restoring force generated as a result of 
bending the finger away from the equilibrium configuration. 

Force- and position-controlled tendons can also be combined, as illus¬ 
trated in the exercises. 


Example 6.5. Coupling matrix for a finger with elastic tendons 

Consider the example shown in Figure 6.10. The extension functions are 
given by 

hi = ll +7-1101 - 7-1202 
/l2 = h ~ 7-2101 

h-3 = I 3 + 7-3101 

hi=l± — 7-4101 + 7-4202, 

where r.jj is the radius of the pulley for the ith tendon on the jth joint. 
The coupling matrix is 

7-11 - 7-21 7-31 - 7-41 

-7-12 0 0 r 42 

Since all of the extension functions are linear, the coupling matrix is 
constant. 

To compute the relationship between the actuator position and the 


joint torques, we use the stiffness matrix 



£1 

0 

0 

o' 

K = 

0 

£2 

0 

0 

0 

0 

£3 

0 


0 

0 

0 

£4 


- 


where ki > 0 is the stiffness of the ith tendon. The overall stiffness is 
given by 


5 ( 0 ) 


PK(h(0) - h{ 0)) 

fci7n + £27-21 + £37-31 + £47-41 
— £l 7 -ll 7 -i 2 - £47-417-42 


— £1 7 - 117-12 - £47-417-42 
£l7"i 2 + £ 47-42 


0 , 


and the coupling matrix between the actuator extension and the joint 
torques is 


Q = PK = 


kirn 

-k\r 12 


-£27-21 £37-31 -£47-41 

0 0 £ 47-42 


4.3 Analysis and control of tendon-driven fingers 

One peculiarity of using tendon networks is that all tendon tensions must 
be strictly positive. Hence, the set of all torques which can be applied 
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is given by the positive span of the columns of P{&). This is analogous 
to grasping using frictionless point contacts, and the same tools can be 
applied to analyze the kinematic properties of the tendon network. 

A control law for a tendon-driven robot computes the joint torques 
r £ R" which must be generated by applying forces to the tendons. We 
say that a tendon network is force-closure if for any r £ R" there exists 
a set of forces / £ R p such that 

P(0)f = r and fi>0,i = l,...,p. (6.34) 

As in the grasping case, a necessary and sufficient condition for a tendon 
network to be force-closure is that P{9) be surjective and there exist a 
strictly positive vector of internal forces /n £ R p , /jv,i > 0 such that 

P(0)f N = o. 

Limits on the number of tendons necessary to construct a force-closure 
tendon network are given by Caratheodory’s and Steinitz’s theorems, 
which were given in Chapter 5, Section 4. Caratheodory’s theorem asserts 
that for a robot with n links, at least n + 1 tendons are required to 
actuate it, while Steinitz’s theorem proves that any more than 2 n tendons 
are redundant. In fact, these two bounds correspond to the two most 
common types of tendon networks, referred to as “TV + 1” and “27V” 
tendon configurations. The 7V+1 configuration usually consists of a single 
tendon which pulls on all of the joints in one direction, together with n 
additional tendons which generate torques in the opposite direction. The 
27V configuration is the one used in all of the examples here, where we 
attach two tendons to each joint, acting in opposite directions. 

For tendon networks which are actuated by force-controlled devices, 
the tendon forces chosen to exert a given vector of joint torques have the 
form 

f = P+(e)r + f N , 

where P + = P T (PP T ) _1 £ R mxn is the pseudo-inverse of the coupling 
matrix and /jv £ A/”(P) (~1 R(j_ is an internal force that insures that all 
tendon tensions are positive. In most situations, /jv will be chosen as 
small as possible, so that the tendons remain taut but are not subjected 
to excessive internal forces. 

The case of elastic tendons with position-controlled actuators is han¬ 
dled exactly the same way, except that we must solve 

P{6)Ke = t and ei + 7q(0) — /q(0) > 0, i = 1,... ,p. (6.35) 

Since K £ R pxp is an invertible stiffness matrix, if the tendon network is 
force-closure, then there exists a vector of extensions ejv £ R p such that 
ejv,i >0 and PKeN = 0. By choosing 

e = ( PK) + t + eN 
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with 6jv sufficiently large in magnitude, we can insure that the constraints 
in equation (6.35) are satisfied. 

The tools presented here can be extended to the case of mixed net¬ 
works of rigid and elastic tendons and also to tendons which extend be¬ 
tween two links in the robot. These cases are explored in the exercises. 

5 Control of Robot Hands 

In this section, we concentrate on the control aspects of multifingered 
robot hands and show how to extend previous controllers (presented in 
Chapter 4) to apply to grasping and other coordinated manipulation 
tasks. In addition, we include some thoughts on organization of complex 
controllers motivated in part by the type of control mechanisms found in 
biological motor control systems. 

5.1 Extending controllers 

For a constrained manipulation problem, we can break the control prob¬ 
lem into two main parts: 

1. Tracking a given object (or workspace) trajectory 

2. Maintaining a desired internal force 

Under the assumption that all objects and links are rigid and their geome¬ 
tries completely known, these two problems can be partially decoupled. 
We first find joint torques which satisfy the tracking requirement and 
then add sufficient internal forces to keep the contact forces inside the 
appropriate friction cones or satisfy some other force objective. 

More specifically, suppose that we have a constrained robotic system 
with dynamics of the form 

M{q)x + C(q, q)x + N(q, q) = F = GJ~ t t , 

with q = (9,x) £ K” x M p . As we saw in the first part of this chapter, 
a large class of systems can be modeled by equations of this form with 
GJ~ t : K" —> a surjective map. In this framework, ieR p represents 
the position variable and the null space of G corresponds to the internal 
force directions (assuming J is invertible). In addition to the general 
form of the dynamics, we also assume that M(q) >0 for all q and that 
M — 2 C is a skew-symmetric matrix. These properties hold for all of the 
systems given in this chapter with the proper definition of J and G. 

The tracking problem is to find joint torques which cause the system 
to asymptotically track a given workspace trajectory Xd(-)- To solve 
this problem, we begin by treating F as a direct input to the system. 
Since GJ~ T is surjective, it is always possible to find a set of torques r 
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which realizes this virtual input. Furthermore, since M and C satisfy the 
properties of Lemma 6.1, we can use any of the controllers derived for 
open-chain manipulators to asymptotically track a trajectory x d (t). 

As an example, the computed torque controller for a constrained robot 
system becomes 

F = M(q) ( x d - K v e - K p e) + C(q,q)x + N(q,q), 

where e := x — x d . Note that in this controller M(q) depends on both 9 
and x. In the special case that the system constraints arc holonomic, the 
dependence on 9 can be removed, but this usually involves inverting a 
nonlinear map. However, since the only essential properties required by 
the proofs of convergence are that M(q) be positive definite and M — 2 C 
skew-symmetric, all of the previous proofs apply directly. 

Once F has been determined, the joint torques r are chosen so that 
GJ~ t t = F. It is always possible to find some such r, since by assump¬ 
tion GJ~ t is surjective. However, GJ~ T is not necessarily injective and 
hence there may be many values of r which generate F. In fact, since J is 
taken to be invertible, the extra freedom in r corresponds to the existence 
of internal forces in the system. The general solution to GJ~ t t = F has 
the form 

r = J T G + F+J T f N , (6.36) 

where G + = G T (GG T )~ 1 is the pseudo-inverse of G and /jv £ A/"(G). 
Since Gfa = 0, /jv can be chosen arbitrarily without affecting the tra¬ 
jectory tracking characteristics of the controller. 

The extra freedom in r is used to satisfy the second part of the control 
problem, regulating internal forces. For a grasping problem, /A must be 
chosen such that the net contact force lies in the friction cone FC. This is 
an extremely important condition, since our entire problem formulation 
assumed that the fingers remained in contact with the object (in the 
directions specified by the contact model). For other types of problems, 
such as coordinated lifting, the regulation of the internal forces is not 
quite so critical, since the mechanical structure will act to enforce the 
constraints at all times. In this case we often choose /jv = 0 to minimize 
application of internal forces. 

In situations in which regulation of internal forces is desired, a fur¬ 
ther complication arises from the fact that the net contact force is not 
given simply by G + F + /jy. Rather, this is the contact force due to the 
actuators. Additional contact forces may be generated by the dynam¬ 
ics of the system, as discussed in Section 2.2. To truly insure that the 
contact forces remain inside the friction cone, the full dynamics must be 
taken into account. This can be done either by computing the forces of 
constraint or measuring them. 

Fortunately, for many problems a detailed analysis of the internal 
forces is not necessary. If the forces of constraint due to non-quasistatic 
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motion are small compared to the desired internal force, they can often 
be ignored. We make that assumption here and assume that we are given 
a desired internal force /jv,d(’) which we wish to regulate and that the 
dynamic forces of constraint can be ignored. In this case the final control 
law has the form 


T — JhG + F + Jfr fN,d 


where F is the virtual force generated to satisfy the trajectory tracking 
requirement. 

A second possibility for controlling internal forces is to measure the 
applied internal forces and adjust /tv using a second feedback control 
law. This must be done carefully since, for a rigid robot system, forces 
are transmitted instantaneously and have no associated dynamics. This 
can lead to problems in which the control problem is ill-posed due to the 
existence of algebraic loops. For example, consider a “robot” which only 
applies forces. Let / be the applied force and fd the desired force. A 
proportional controller regulating the force is given by 


/ = <*(/- fd)- 


For a ^ 0, this controller only satisfies / = fd if / = fd = 0. For any 
other fd, we have an algebraic loop which is never satisfied. 

One common way to overcome this problem is to use an integrator. 
Again we let f d be the desired force and set 



This controller consists of a feedforward piece, f d , and an integrating 
compensator. Setting f = f d generates no contradictions, and hence this 
controller is well-posed. It is important to note that adding an integrator 
must be done carefully to avoid introducing unstable behavior into the 
system due to measurement noise and “integrator-windup.” Details of 
these problems can be found in standard undergraduate texts on control 
engineering, such as [3] and [34]. 

5.2 Hierarchical control structures 

A multifingered robot hand can be modeled as a set of robots which are 
coupled to each other and an object by a set of velocity constraints. The 
analysis presented in the beginning of this chapter allows us to model this 
interconnection and create a new dynamical system which incorporates 
the constraints. In fact, this procedure is sufficiently straightforward that 
it may be automated: by specifying the contact constraint between the 
robots and the object, the new equations of motion for the composite 
robot can be derived using a symbolic manipulation program. 
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A significantly more difficult problem is that of constructing con¬ 
trollers for robot systems. Although conceptually simple, a controller 
for a multifingered robot hand must be able to control a very complex 
system with many degrees of freedom, large amounts of sensory data, and 
multiple control objectives. A typical hand might have 10-20 actuators, 
10-15 constraints, and a state-space of dimension 30 or higher. A control 
law for such a system might need to run at a control frequency of 500 Hz 
or more in order to yield acceptable performance. Computing the control 
torques for such a system in under 2 milliseconds is often impossible if 
the system is modeled as a single, complex dynamical system. 

The difficulties in controlling systems with many degrees of freedom 
have also been noted in the biomechanics literature. The study of hu¬ 
man biological motor control mechanisms led the Russian psychologist 
Bernstein to question how the brain could control a system with so many 
different degrees of freedom interacting in such a complex fashion [41]. 
Many of these same complexities are also present in robotic systems and 
limit our ability to use multifingered hands and other robotic systems to 
their full advantage. 

In the remainder of this section, we describe one possible way of struc¬ 
turing controllers which attempts to address some of the difficulties in¬ 
herent in the control of constrained robot systems with many degrees of 
freedom. We present a set of primitive operations that allow a complex 
robot controller to be built up in a hierarchical fashion and discuss some 
of the issues involved in the resulting control structure. The material 
contained in this section was originally presented in [75], where a more 
detailed description is given. 

Defining robots 

We wish to build up complex control laws by utilizing the geometric 
constraints between the mechanisms which make up the overall system. 
We will model all mechanisms as a generalized object which we label as a 
robot. A robot consists of a dynamical system whose equations of motion 
have the form 

M(q)q + C(q,q)q + N(q,q)=F. 

The quantities M, C, and N completely parameterize the dynamics of 
the mechanism. 

In addition to the parameters (M, C, N ), a robot also has a set of 
inputs and outputs. The inputs consist of the desired position of the 
robot, Xd, and the forces to be applied to the robot, Fd- The outputs are 
the actual position of the mechanism, x , and the measured force F. Some 
types of robots may not use or define all of these inputs and outputs. 

The relationship between the various parameters describing a robot 
depends on the robot itself. For example, we model an actuated, open- 
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chain mechanism using the relationships 


F = F d 

M(q)q + C(q,q)q + N(q,q) = F. 

Thus, given a desired force to be applied to the robot, the robot will move 
according to the equations of motion. The desired trajectory input for 
an uncontrolled robot is ignored. (We will make use of this input later 
when we attach controllers to robots.) 

In addition to modeling actuated mechanisms, a robot can also de¬ 
scribe an inanimate object. In this case, all inputs to the robot are 
ignored and the outputs from the robot provide information about the 
current position of the object and the forces acting on it, if available. The 
inertial parameters (M, C, N) are used as before to model the dynamics 
of the object. 

The utility of defining a generalized object called a robot is that we 
may define operations which take one or more generalized robots and 
yield a new generalized robot. We define two such operations below. In 
order to define the new object, we must define the inertial properties 
as well as a description of the inputs and outputs. These are typically 
defined recursively, so that a composite robot queries and commands its 
children in response to requests for inputs and outputs. 


Attaching robots 

The first operation which we define is the attach operation, which reflects 
geometrical constraints between two or more robots. It creates a new 
robot object from the attributes of its children. Its definition (and name) 
is motivated by the attachment of a set of fingers to an object, but its 
use is much more general. 

The arguments to the attach operation are a list of robots, which we 
refer to as fingers, together with a single object which we refer to as the 
payload. In addition, we are given a constraint between the configuration 
variables of the fingers and those of the object. For simplicity, we take 
this constraint to be of the form h(6, x) = 0 where 9 = (9 ^,..., 6 f k ) & R n 
is the vector of finger joint angles and x € is the configuration of the 
payload. 

To construct a new robot, we use the Lagrange-d’Alembert equa¬ 
tions to write the dynamics in terms of the payload variables x £ 

Let ( Mf,Cf,Nf ) be the (block-diagonal) parameters for the fingers and 
( M p: C pi N p ) be those of the payload. The dynamic parameters for the 
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constrained robot are given by 

M := M p + GJ~ T M f J~ 1 G 
C := C p +GJ- T C f J- 1 G + GJ- T M f ^ t (J^G) 

N := N p + GJ~ T Nf, 

where J = G T = — and we assume that J is invertible for the 
purposes of exposition. Note that in order to evaluate M at the current 
configuration, we can query the payload and each of the fingers for their 
current inertia matrices and then combine these using the constraints. 

To read the configuration of the composite robot, we query the state 
of all the robots in the list of daughter robots and then solve the (holo- 
nomic) constraint h(0, x) = 0 to find the current payload configuration. 
Alternatively, if the payload is equipped with sensors (perhaps an exter¬ 
nal camera which tracks the payload), this data can be used instead. A 
similar computation or measurement can be used to determine the net 
force on the object, which will consist of contact forces applied by the 
fingers and external forces applied by the environment. 

Commanding the desired position and force on the robot also uses 
the constraint equations to distribute information to the fingers and the 
payload. If all fingers are uncontrolled, actuated mechanisms, then the 
desired forces will be applied to the actuators and the desired position 
will be ignored. 

A diagram illustrating the data flow in a robot constructed by the 
attach operation is shown in Figure 6.11. In addition to modeling grasp 
constraints, the attach operation can be used to model other situations, 
as described in Section 2.3. For example, we can change from joint space 
to workspace coordinates or add variables parameterizing the internal 
motion of a redundant robot. 

Controlling robots 

The control operation is responsible for assigning a controller to a robot. 
It is also responsible for creating a new robot with attributes that properly 
represent the controlled robot. The attributes of the created robot are 
completely determined by the individual controller. For most controllers, 
the current state of the controlled robot is equivalent to the current state 
of the uncontrolled robot. Sending a desired trajectory to a controlled 
robot would cause the controller to buffer the data and attempt to follow 
that trajectory. A controlled robot is illustrated in Figure 6.12. 

The dynamic attributes M 1 C , and N for the newly created robot 
are determined by the controller. At one extreme, a controller which 
compensates for the inertia of the robot would set the dynamic attributes 
of the controlled robot to zero. This does not imply that the robot is 
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Figure 6.11: Data flow between two robots which have been attached to 
a payload. 


Xd 
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Figure 6.12: Data flow in a typical controlled robot. 


no longer a dynamic object, but rather that controllers at higher levels 
can ignore the dynamic properties of the robot, since they are being 
compensated for at a lower level. At the other end of the spectrum, a 
controller may make no attempt to compensate for the inertia of a robot, 
in which case it should pass the dynamic attributes on to the next higher 
level. Controllers which lie in the middle of this range may partially 
decouple the dynamics of the manipulator without actually completely 
compensating for them. 

Building hierarchical controllers 

The operation described above allow us to build complex hierarchical con¬ 
trol laws for robot systems. Since a controller accepts a robot object and 
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Figure 6.13: Hierarchical control scheme for a human finger. (Figure 
courtesy of D. Curtis Deno) 


creates a robot object, it can be inserted at any level in the description of 
a constrained robot system. Thus, we can easily define hierarchical con¬ 
trollers whose structure mirrors the geometric structure of the system. 
We illustrate this in the following examples. 

Example 6.6. Biological motor control 

Figure 6.13 shows a hierarchical control scheme for a human finger. At 
the highest level, the brain is represented as sensory and motor cortex 
(where sensory information is perceived and conscious motor commands 
originate) and brainstem and cerebellar structures (where motor com¬ 
mands are coordinated and sent down the spinal cord). A pair of fingers 
forms a composite system for grasping which is shown integrated at the 
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Figure 6.14: Planar two-fingered hand and a hierarchical control law. 

level of the spinal cord. The muscles and sensory organs of each finger 
form low-level spinal reflex loops. These low-level loops respond more 
quickly to disturbances than sensory motor pathways which travel to the 
brain and back. Brain and spinal feedback controllers are represented by 
double-lined boxes. 

The block diagram portion of Figure 6.13 is a (biological) example of 
a robot system built using the operations described above. Starting from 
the bottom: two fingers (robots) are defined; each finger is controlled by 
muscle tension/stiffness and spinal reflexes; the fingers are attached to 
form a composite hand; the brainstem and cerebellum help control and 
coordinate motor commands and sensory information; and finally, at the 
level of the cortex, the fingers are thought of as a pincer which engages 
in high-level tasks such as picking. 

Example 6.7. Hierarchical control of a two-fingered planar hand 

As a second, more practical example, consider the planar hand shown in 
Figure 6.14. The parameters describing the dynamics of the fingers and 
the object, as well as the constraints between them, are easily computed 
and are given in earlier examples. We can build a hierarchical controller 
which is similar to the biological controller described in the previous 
example. This control structure is shown graphically in Figure 6.15. 

At the lowest level, we use simple PD control laws attached directly to 
the individual fingers. These PD controllers mimic the stiffness provided 
by muscle coactivation in a biological system. Additionally, controllers at 
this level might be used to represent spinal reflex actions. At a somewhat 
higher level, the fingers are attached and considered as a single unit with 
relatively complicated dynamic attributes and Cartesian configuration. 
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Figure 6.15: A hierarchical controller for multifingered grasping. 


At this point, we employ a feedforward controller (computed torque with 
no error correction) to simplify these dynamic properties, as viewed by 
higher levels of the brain. With respect to these higher levels, the two 
fingers appear to be two Cartesian force generators, represented as a 
single composite robot. 

Up to this point, the representation and control strategies do not 
explicitly involve the box, a payload object. The force generators are 
next attached to the box, yielding a robot with the dynamic properties 
of the box, but capable of motion due to the actuation in the fingers. 
Finally, we use a computed torque controller at the very highest level 
to allow us to command motions of the box without worrying about the 
details of muscle actuation. By this controller, we simulate the actions 
of the cerebellum and brainstem to coordinate motion and correct for 
errors. 

It is helpful to illustrate the flow of information to the highest level 
control law. In the evaluation of the current box configuration and tra- 
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jectory, Xb and ib, the following sequence of actions occurs: 

Hand: asks for current state, Xb and Xb 

Finger: ask for current state, x/ and if 
Left: read current state, 0/ and 0; 

Right: read current state, 9 r and 0,- 
Finger: Xf,if <- f(9i,9 r ),J{9i,6 r ) 

Hand: Xb, ib g{xf), G +T if. 

When we write a set of hand forces, a similar chain of events occurs. 

The structure in Figure 6.15 also has interesting properties from a 
more traditional control viewpoint. The low-level PD controllers can be 
run at high servo rates (due to their simplicity) and allow us to tune 
the response of the system to reject high-frequency disturbances. The 
Cartesian feedforward controller permits a distribution of the calculation 
of nonlinear compensation terms at various levels, lending itself to multi¬ 
processor implementation. Finally, using a computed torque controller at 
the highest level gives the flexibility of performing the controller design 
in the task space and results in a system with linear error dynamics. 
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6 Summary 


The following are the key concepts covered in this chapter: 

1. The dynamics of a mechanical system with Lagrangian L(g, q ), sub¬ 
ject to a set of Pfaffian constraints of the form 

A(q)q = 0 A(q) G R kxn , 


can be written as 

d dL 
dt dq 


dL 

dq 


+ A T (q) A-T = 0, 


where A G is the vector of Lagrange multipliers. The values of 
the Lagrange multipliers are given by 

A = (AM~ 1 A t )~ 1 ( AM~\F — Cq — N) + Aqj . 


2. The Lagrange-dAlembert formulation of the dynamics represents 
the motion of the system by projecting the equations of motion 
onto the subspace of allowable motions. If q = (< 71 ,( 72 ) G R( ra “ fc ) xfe 
and the constraints have the form 


<72 = A(q)qi, 


then the equations of motion can be written as 


(±dL_dL_ \ T (±dL_ 
\dtdqi dq± 1 ) \dt dqi 



= 0 . 


In the special case that the constraint is integrable, these equations 
agree with those obtained by substituting the constraint into the 
Lagrangian and then using the unconstrained version of Lagrange’s 
equations. 


3. The dynamics for a multifingered robot hand with joint variables 
8 G R” and (local) object variables x G K p , subject to the grasp 
constraint 

Jh(0,x)6 = G t (6,x)x, 

is given by 

M(q)x + C(q , q)x + N(q, q) = F, 
where <7 = ( 0 , x) and 

M = M 0 + GJ- T M f J- x G T 

N = N a + GJ~ T N f 
F = GJfi T r. 


C = C 0 + GJfi T [c f J^G T + M f j f (,Jfi 1 G T ) 
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These same equations can be applied to a large number of other 
robotic systems by choosing G and Jh appropriately. 

4. For redundant and/or nonmanipulable robot systems, the hand Ja¬ 
cobian is not invertible, resulting in a more complicated derivation 
of the equations of motion. For redundant systems, the constraints 
can be extended to the form 


Jh 

9 = 

G T 

O' 


X 

Kh 

0 

I 


y N _ 


Jh G T 

where the rows of Kh span the null space of Jh , and vn represents 
the internal motions of the system. For nonmanipulable systems, 
we choose a matrix H which spans the space of allowable object 
trajectories and write the constraints as 

J h 9 = G^Hw, 

QT 

where x = H(q)w represents the object velocity. In both the re¬ 
dundant and nonmanipulable cases, the augmented form of the con¬ 
straints can be used to derive the equations of motion and put them 
into the standard form given above. 

5. The kinematics of tendon-driven systems are described in terms 
of a set of extension functions , hi : Q — > R, which measures the 
displacement of the tendon as a function of the joint angles of the 
system. If a vector of tendon forces / £ R fe is applied at the end of 
the tendons, the resulting joint torques are given by 

T = P{0)f, 

where P(9) £ R nxp is the coupling matrix: 

m = § V 

A tendon-system is said to be force-closure at a point 9 if for every 
vector of joint torques, r, there exists a set of tendon forces which 
will generate those torques. 

6 . The equations of motion for a constrained robot system are de¬ 
scribed in terms of the quantities M(q ), C(q, q), and N(q, q). When 
correctly defined, the quantities satisfy the following properties: 

(a) M(q) is symmetric and positive definite. 

(b) M(q) — 2 C is a skew-symmetric matrix. 
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Using these properties it is possible to extend the controllers pre¬ 
sented in Chapter 4 to the more general class of systems considered 
in this chapter. For a multifingered hand, an extended control law 
has the general form 


T = J T G+F+J T f N , 

where F is the generalized force in object coordinates (determined 
by the control law) and fw is an internal force. The internal forces 
must be chosen so as to insure that all contact forces remain inside 
the appropriate friction cone so that the fingers satisfy the funda¬ 
mental grasp constraint at all times. 
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general case of actuator networks with compliance and internal loops. 
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8 Exercises 


1. Calculate the dynamics of a spherical pendulum using the Lagrange- 
d’Alembert equations and check that they agree with the result 
derived in Example 6.1. 

2. For the rolling disk in Example 6.2, show that substituting the 
constraints on x and y into the Lagrangian and then applying La¬ 
grange’s equations (without constraints) gives the wrong equations 
of motion. 


3. Calculate the constraint forces for the rolling penny described in 
Example 6.2. 

4. Structural properties of the Newton-Euler equations 

Consider the Newton-Euler equations for the motion of a rigid body: 

ml 0 
0 1 


uj° x mv b 
cu b x lLV b 


= F b . 


Let x 0 = (p, R) £ SE( 3) denote the configuration of the rigid body 
and V b = ( v b ,L 0 b ) = (x~ 1 x 0 ) v £ M 6 the body velocity. 


(a) Show that the Newton-Euler equations can be written as 


M{x 0 )V b + C(x 0 ,x 0 )V b = F b , 


where M(x 0 ) > 0 and M — 2C is a skew-symmetric matrix. 

(b) Let x a = <j>{x), x £ R 6 be a local parameterization of x Q (using 
Euler angles to represent rotation, for example). Assuming 
that the matrix 




£ R 


6x6 


is nonsingular, show that the equations of motion become 
M(x)x + +C(x, x)x = J T (x)F b , 


where 

M = J t MJ 
C = J T CJ + J t MJ. 

5. Derive the dynamics of the four-bar linkage shown below in terms 
of the crank angle Q\ £ S 1 . 
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6 . Derive the actuator kinematics for the tendon networks shown be¬ 
low. Consider both the rigid and elastic cases. 


(a) 

7. Consider the two-link planar manipulator shown below. 





(a) Calculate the kinematic constraints on the system. 

(b) Calculate the dynamics of the manipulator assuming that the 
end-effector remains in contact with the wall at x = c. Model 
the inertia of each link as a point mass concentrated at the 
middle of the link. 

(c) Calculate the internal forces due to the manipulator dynamics. 

(d) Design a PD controller which gives asymptotic tracking of tra¬ 
jectories in the y direction while pushing with constant force 
fd> 0 in the x direction. 
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Chapter 7 

Nonholonomic Behavior 
in Robotic Systems 


In this chapter, we study the effect of nonholonomic constraints on the 
behavior of robotic systems. These constraints arise in systems such 
as multifingered robot hands and wheeled mobile robots, where rolling 
contact is involved, as well as in systems where angular momentum is 
conserved. We discuss the problem of determining when constraints on 
the velocities of the configuration variables of a robotic system are inte¬ 
grate, and illustrate the problem in a variety of different situations. The 
emphasis of this chapter is on the basic tools needed to analyze nonholo¬ 
nomic systems and the application of those tools to problems in robotic 
manipulation. These tools are drawn both from some basic theorems in 
differential geometry and from nonlinear control theory. 


1 Introduction 

In the preceding chapter, we derived the equations of motion for a robotic 
system with kinematic constraints. We restricted ourselves to Pfaffian 
constraints which had the general form 

J{9,x)6 = G t (0,x)x, (7.1) 

where q = (0, x ) £ R" is the configuration of the system. As we saw, 
equations of this form could be used to model a large number of robotic 
systems, including multifingered hands, robots in contact with their en¬ 
vironment, and redundant manipulators. 

By shifting our notation slightly, we can write the preceding con¬ 
straints in the form 


tOi(q)q = 0 i = l,...,k, 


(7.2) 
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where the u>i(q) are row vectors. We assume that the u>i are linearly 
independent at each point q € R", since if they are not, the dependent 
constraints may be eliminated. Each u>i describes one constraint on the 
directions in which q is permitted to take values. 

Recall from Chapter 6 that a constraint is said to be holonomic if it 
restricts the motion of a system to a smooth hypersurface of the configu¬ 
ration space. It will be convenient to adopt some language and notation 
from differential geometry, so we call this smooth hypersurface a manifold. 
Locally, a holonomic constraint can be represented as a set of algebraic 
constraints on the configuration space, 

hi(q) = 0, i = l,...,k. (7.3) 

The dimension of the manifold on which the motion of the system evolves 
is n — k. 

We say that a set of k Pfaffian constraints of the form in equation (7.2) 
is integrable if there exist functions hi : R" — > R, * = 1,..., k such that 

hi(q(t)) = 0 Ui{q)q = 0 i = l,...,k. 

Thus, a set of Pfaffian constraints is integrable if it is equivalent to a set 
of holonomic constraints. We often call an integrable Pfaffian constraint 
a holonomic constraint, although strictly speaking the former is described 
by a set of velocity constraints and the latter by a set of functions. A set 
of Pfaffian constraints is said to be nonholonomic if it is not equivalent 
to a set of holonomic constraints. 

As we saw in Chapter 6 , the presence of nonholonomic constraints 
requires special care in deriving the equations of motion for the system. 
The point of view taken in this chapter is somewhat different. Here, we 
will try to understand when we can exploit the nonholonomy of the con¬ 
straints to achieve motion between configurations. In particular, we will 
be interested in answering the following question: given two points qo 
and qf , when does there exist a path q(t) which satisfies the constraints 
in equation (7.2) at all times and connects qo to q/? The set of all points 
which can be connected to qo via a path which satisfies the constraints is 
called the reachable set associated with q 0 . Thus, we wish to understand 
under what conditions the reachable set will be the entire configuration 
space. This is intimately related to the nonholonomy of the constraints, 
since if the constraints are holonomic, then the motion of the system is 
restricted to the level sets given by hi(q) = hi(qo), i = 1,... ,k. Hence, 
for holonomic constraints the reachable set is some subset of the config¬ 
uration space which lies in the level set /q(g) = hi(qo), and we cannot 
move freely between configurations on different level sets. 

A good example of the type of behavior which we wish to exploit is 
that of an automobile. The kinematics of an automobile are constrained 
because the front and rear wheels are only allowed to roll and spin, but 
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not to slide sideways. As a consequence, the car itself is not capable 
of sliding sideways, or rotating in place. Despite this, we know from 
our own experience that we can park an automobile at any position and 
orientation. Thus, the constraints are not holonomic since the motion 
of the system is unrestricted. Finding an actual path between two given 
configurations is an example of a nonholonomic motion planning problem 
and is the subject of the next chapter. 

Checking to see if a constraint is holonomic or nonholonomic is neither 
easy nor obvious. Consider first the case in which there is a single velocity 
constraint, 

n 

w(<7)<7 = J2^j{q)qj = 0. 

3 =1 

This constraint is integrable if there exists a function h : R" —> R such 
that 

ui{q)q = 0 <*=>■ h{q) = 0. 

It follows by differentiating h(q) = 0 with respect to time that if the 
PfafRan constraint is holonomic then 

” n Fth 

J2 = 0 =► J2 = 0 - 

j = 1 j =1 Uq 3 


In turn, this implies that there exists some function a(q), called an inte¬ 
grating factor , such that 

a{q)uj{q) = J^(?) j = (7.4) 


Thus, a single Pfaffian constraint is holonomic if and only if there exists 
an integrating factor a(q) such that a(q)u>(q) is the derivative of some 
function h. 

Equation (7.4) is not very constructive from the point of view of check¬ 
ing integrability since it involves the unknown function h(q). This situa¬ 
tion may be remedied by using the fact that 


d 2 h _ d 2 h 

dqidqj dqjdqi 


to get 


d(aujj) d{au>i) 

dqi dq 3 


i,j = !,-■■,n. 


(7.5) 


Equation (7.5) states that the constraint is equivalent to h(q) = 0 if there 
exists some integrating factor a(q) for which the equation (7.5) is true. 
This should really not be a surprise since 


u{q)q = 0 


a(q)u>(q)q = 0 
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for all choices of smooth functions a(q). However, one still has to find a 
function a which satisfies equation (7.5). 

The question of integrability becomes much more difficult in the pres¬ 
ence of multiple Pfaffian constraints. Given a set of k constraints of the 
form of equation (7.2), not only does one need to check whether each 
one of the k constraints is integrable, but also which independent linear 
combinations of these, 

k 

'Y^a. i (q)u i (q)q, 

i=l 

are integrable. That is, even if the given constraints are not individually 
integrable, they may contain a set of integrable constraints. Thus, there 
may exist functions hi for i = 1 ,... ,p with p < k such that 


r dhi . . dh p . .. 
span{ — {q),...,-^(q)} 


C spanjuq^),... ,w fc (g)} 


for all q. If it is possible to find these functions, the motion of the system 
is restricted to level surfaces of h, namely to sets of the form 


{q : hi(q) = c u ...,h p (q) = c p }. 

If p = k, then the constraints are holonomic. In the case that p < k, the 
constraints are not holonomic (since they are not completely equivalent 
to a set of holonomic constraints) but the reachable points of the system 
are still restricted. Thus the constraints are “partially holonomic.” We 
will be primarily interested in the case in which the constraints do not 
restrict the reachable configurations. We refer to this situation as being 
completely nonholonomic. 

It will be convenient for us to convert problems with nonholonomic 
constraints into another form. Roughly speaking, we would like to ex¬ 
amine the systems not from the point of view of the constraints (namely, 
the directions that we cannot move), but rather from the viewpoint of 
the directions in which we are free to move. We begin by choosing a 
basis for the right null space of the constraints, denoted by gj(q) £ R n , 
i = 1,... ,n — k =: m. By construction, this basis satisfies 


Ui{q)gj(q) = 0 


i = 1 ,..., k 
j = k, 


and the allowable trajectories of the system can thus be written as the 
possible solutions of the control system 

q = gi(q) Ul -i - b gm(q)u m - (7.6) 

That is, q(t) is a feasible trajectory for the system if and only if q(t) 
satisfies equation (7.6) for some choice of controls u(t) £ R m . 
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In this context, a constraint is completely nonholonomic if the corre¬ 
sponding control system can be steered between any two points. Thus 
the reachable configurations of the system are not restricted. Conversely, 
if a constraint is holonomic, then all motions of the system must lie on 
an appropriate constraint surface and the corresponding control system 
can only be steered between points on the given manifold. Hence, we can 
study the nature of Pfaffian constraints by studying the controllability 
properties of equation (7.6). 

Nonholonomic constraints arise in a variety of applications. Besides 
rolling constraints on multifingered hands, nonholonomic constraints play 
an important role in the study of mobile robot systems and space-based 
robotic systems (in which conservation of angular momentum plays the 
role of a nonholonomic constraint). For these applications the primary 
question is that of reachability: when can we find a path between two 
arbitrary configurations and how do we go about computing such a path? 

The outline of this chapter is as follows: in Section 2 we develop some 
tools from differential geometry and nonlinear control. Section 3 gives 
examples of systems with velocity constraints. In Section 4 the structure 
of nonholonomic systems is explored and the examples of Section 3 are 
analyzed. In the next chapter, we will develop methods for planning 
paths compatible with nonholonomic constraints. 

Both this chapter and Chapter 8 are slightly more advanced in flavor 
than the previous chapters and represent some of the recent research in 
the robotics literature. Nonholonomic behavior also plays a strong role in 
many problems in geometric mechanics, which we touch on only briefly 
in the examples and exercises. In classical mechanics, nonholonomic be¬ 
havior is closely related to the geometric phase associated with a group 
symmetry in a Hamiltonian or Lagrangian system. A good introduction 
to these concepts can be found in the lecture notes by Marsden [67]. 

2 Controllability and Frobenius’ Theorem 

In the previous section, we saw the difficulties in trying to determine 
whether or not constraints on a system were holonomic (or integrable). 
Further, if they are not holonomic, it is not completely clear as to when 
they are completely nonholonomic. In this section, we will develop the 
machinery needed for analyzing nonholonomic systems, in particular for 
answering the question of when a set of Pfaffian constraints is holonomic. 

The tools we develop are based on a variety of results from differential 
geometry and nonlinear control theory, more specifically Frobenius’ theo¬ 
rem and nonlinear controllability. To keep the mathematical prerequisites 
to a minimum, we do all the calculations in R” and restrict ourselves to 
drift-free control systems (i.e., control systems whose state remains fixed 
when the input is turned off). Many of the proofs in this section rely on 


321 


some properties of manifolds which we have omitted from the discussion; 
they can be skipped without loss of continuity. A good introduction to 
nonlinear control theory which includes many of the necessary differential 
geometric concepts can be found in Isidori [43] or Nijmeijer and van der 
Schaft [83]. 


2.1 Vector fields and flows 


We restrict our attention to R". We choose to make a distinction, how¬ 
ever, between the space and its tangent space at a given point. A point 
of contact with Chapter 2 is our insistence there on making a distinction 
between points and vectors in R 3 and enforcing the distinction by aug¬ 
menting points by 1 and vectors by 0. Denote by T q R” the tangent space 
to R n at a point q £ R™. A vector field on R" is a smooth map which 
assigns to each point q £ R" a tangent vector f(q) £ T q R”. In local 
coordinates, we represent / as a column vector whose elements depend 
on q , 

/i(?) 


/(«) = 


fn(q)_ 


A vector field is smooth if each fi(q) is smooth. 

Vector fields are to be thought of as right-hand sides of differential 
equations: 

<? = /(?)• ( 7 - 7 ) 


The rate of change of a smooth function V : R" 
is given by 




dqfi 


R along the flow of / 


The time derivative of V along the flow of / is referred to as the Lie 
derivative of V along / and is denoted LfV: 


dV 

L ’ v ■■= w /(,> ' 


Associated with a vector field, we define the flow of a vector field 
to represent the solution of the differential equation (7.7). Specifically, 
<t>{ (q) represents the state of the differential equation at time t starting 
from q at time 0. Thus (f>{ : R" —> R n satisfies 


A vector field is said to be complete if its flow is defined for all t. By the 
existence and uniqueness theorem of ordinary differential equations, for 
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Figure 7.1: A Lie bracket motion. 


each fixed t, <t>{ is a local diffeomorphism of K™ onto itself. Further, it 
satisfies the following group property: 

<t>{ ° ¥ a = ri+s, 

for all t and s, where o stands for the composition of the two flows, namely 

2.2 Lie brackets and Frobenius’ theorem 

Given two vector fields g\ and < 72 , the map (f> f 1 o tp 92 stands for the com¬ 
position of the flow of <?2 for s seconds with the flow of g\ for t seconds. 
In general, this quantity is different from the map <p 92 o q i® 1 , which stands 
for the composition in reverse order. Indeed, consider the flow depicted 
in Figure 7.1 starting from q 0 . It consists of a flow along g\ for e seconds 
followed by a flow along <72 for e seconds, —g± for e seconds, and —<72 for e 
seconds. For e small, we may evaluate the Taylor series in e for the state 
of the differential equation as 

q{e) = 0)) 

= q(0) + em + \t 2 m + O(e 3 ) 

= Qo + e5l(<7o) + 2 

where the notation 0(e k ) represents terms of order e fc and the partial 
derivative of <?i is evaluated at q$. 
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Now evaluating at time 2e, 
q(2e) = <f> 92 o <t>f(q 0 ) 


= <Af (9o + £9i(9o) + ^-^9i(9o) + G(e 3 )) 

1 2 dgi 

= 9o + £3i(9o) + 2 e -Qq 9l ( q °) 

e 2 9 q2 o 

+ £ 92(90 + £9i(9o)) + y -^- 92 ( 90 ) +0(e 3 ) 

= 90 + e(9i(9o) + 92 ( 90 )) 

+ 2 e2 (-§^ 9l ( q °} + 92(90) + 2 ~,j|^9i(9o)) + ^( e3 )- 

Here, we have used the Taylor series expansion for 92(90 + £ 91 ( 9 )) = 
92 ( 90 ) + £y^9i(9o) + 0(e 2 ). At the next step (we invite the reader to 
verify this), we get 


9(3e) = <j) t 91 o0f o^f (g 0 ) 

= 9o + £ 92 ( 90 ) 

+ y (^92(90) + 2^91 (g 0 ) - 2 ^g 2 (g 0 )) + 0 (e 3 )- 

Finally, we get 


g(4e) = </> e 92 o 4> e 91 o </f o (g 0 ) 

= 9° + £ 2 (^9i(9o) - ^92(90)) + 0(e 3 )- 


(7.8) 


Motivated by this calculation, we define the Tie bracket of two vector 
fields / and g as 

[/i 9]( 9 ) = |^/(9) - |^9(9)- 

The Lie bracket is thus the infinitesimal motion (actually of order e 2 ) 
that results from flowing around a square defined by two vector fields 
/ and g. If [/, 9 ] = 0 then it can be shown that the right hand side of 
equation (7.8) is identically equal to go and / and g are said to commute. 
A Lie product is a nested set of Lie brackets, for example, 


[[/> 9]»[/, [/, 9111- 

Example 7.1. Lie brackets of linear vector fields 

Consider two linear vector fields given by /(g) = Ag and g(q) = Bq. 
Then the Lie bracket of the two linear vector fields is a linear vector field 
given by 

[/s 9 ]( 9 ) = (BA-AB)q, 

that is, it is the commutator of the two matrices A, B. 
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The following properties of Lie brackets follow from the definition. 
Their proof is left as an exercise. 

Proposition 7.1. Properties of Lie brackets 

Given vector fields /, g, h on R” and smooth functions a , (3 : R™ —> R, 
the Lie bracket satisfies the following properties: 

1. Skew-symmetry: 

[f,g\ = ~[g,f} 


2. Jacobi identity: 


[/, \g, /i]] + [h, [f, g}} + \g, [h, /]] = 0 


3. Chain rule: 


[a/, Pg] = <*l3[f, g] + a(Lf(3)g - (3{L g a)f , 

where Lf/3 and L g a stand for the Lie derivatives of P and a along 
the vector fields f and g respectively. 

An alternative method of defining the Lie bracket of two vector fields 
/ and g is to require that it satisfies for all smooth functions a : R” —> R: 

L[f,g] a = Lf(Lga) — Lg(Lfa). 

The reader should carefully parse the previous equation and convince 
herself of this fact. 

A distribution assigns a subspace of the tangent space to each point 
in M” in a smooth way. A special case is a distribution defined by a set 
of smooth vector fields, g i,, g m . In this case we define the distribution 
as 

A = span{gq,.. .,g m }, 

where we take the span over the set of smooth real-valued functions on 
R". Evaluated at any point q £ R n , the distribution defines a linear 
subspace of the tangent space 

A q =span{ffi(g),...,g m (g)} C T g M. n . 

The distribution is said to be regular if the dimension of the subspace A q 
does not vary with q. A distribution is involutive if it is closed under the 
Lie bracket, i.e., 

A involutive \/f,geA, [/, g] € A. 

For a finite dimensional distribution it suffices to check that the Lie brack¬ 
ets of the basis elements are contained in the distribution. The involutive 
closure of a distribution, denoted A, is the closure of A under bracketing; 
that is, A is the smallest distribution containing A such that if /, g € A 
then [/, g] <E A. 
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Definition 7.1. Lie algebra 

A vector space V (over R) is a Lie algebra if there exists a bilinear op¬ 
eration V x V —> V, denoted [ , ], satisfying (i) skew-symmetry and (ii) 
the Jacobi identity. 

The set of smooth vector fields on R n with the Lie bracket is a Lie 
algebra, and is denoted X(R”). Let < 71 ,..., g m be a set of smooth vec¬ 
tor fields, A the distribution defined by g±, ... ,g m and, A the involutive 
closure of A. Then, A is a Lie algebra (in fact the smallest Lie al¬ 
gebra containing gi, ■ ■ ■ ,g m )- It is called the Lie algebra generated by 
< 71 ,... ,g m and is often denoted C(g 1 ,... ,g m )- Elements of C{g 1 ,... ,g m ) 
are obtained by taking all linear combinations of elements of g ±,..., g m , 
taking Lie brackets of these, taking all linear combinations of these, and 
so on. We define the rank of £( 31 ,... ,g m ) at a point q £ R ra to be the 
dimension of A q as a distribution. 

A distribution A of constant dimension k is said to be integrable if for 
every point q £ R", there exists a set of smooth functions hi : R n —> R, 
i = 1 ,..., n — k such that the row vectors are linearly independent 
at q , and for every / £ A 

dh' 

Lfhi = -^f(q) =0 i = 1,... ,n — k. (7.9) 

The hypersurfaces defined by the level sets 

{q : hi{q) = Ci,..., h n _ fc (g) = c n _ fe } 

are called integral manifolds for the distribution. If we regard an integral 
manifold as a smooth surface in R", then equation (7.9) requires that 
the distribution be equal to the tangent space of that surface at the point 
Q- 

Integral manifolds are related to involutive distributions by the fol¬ 
lowing celebrated theorem. 

Theorem 7.2 (Frobenius). A regular distribution is integrable if and 
only if it is involutive. 

Thus, if A is an fc-dimensional involutive distribution, then locally 
there exist n — k functions hi: R" —> R such that integral manifolds of 
A are given by the level surfaces of h = (hi, ..., h n -k). These level 
surfaces form a foliation of R". A single level surface is called a leaf of 
the foliation. 

Associated with the tangent space T g R ra is the dual space T g R ra , the 
set of linear functions on T q R". Just as we defined vector fields on R", we 
define a one-form as a map which assigns to each point q £ R" a covector 
ui(q) £ T* R n . In local coordinates we represent a smooth one-form as a 
row vector 

u>(q) = [uj 1 (q) uj 2 (q) ••• ui n (q)] . 
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Differentials of smooth functions are good examples of one-forms. For 
example, if (3 : K’ 1 —> R, then the one-form d/3 is given by 



ap 

dq n 


Note, however, that all one-forms are not necessarily the differentials of 
smooth functions (a one-form which does happen to be the derivative of 
a function is said to be exact). 

A one-form acts on a vector field to give a real-valued function on R” 
by taking the inner product between the row vector to and the column 
vector /: 



A codistribution assigns a subspace of T*R” smoothly to each q € R”. A 
special case is a codistribution obtained as a span of a set of one-forms, 


= span{w 1; .. 


where the span is over the set of smooth functions. As before, the rank 
of the codistribution is the dimension of f l q . The codistribution f l is said 
to be regular if its rank is constant. 

To begin our study of motion planning for nonholonomic systems, 
our first task is to convert the specified constraints given as one-forms 
into an equivalent control system. To this end, consider the problem of 
constructing a path q(t) £ R" between a given q 0 and qj subject to the 
constraints 


Ui(q)q = 0 i = 


The LOi s are linear functions on the tangent spaces of R", i.e., one-forms. 
We assume that the uif s are smooth and linearly independent over the 
set of smooth functions. The following proposition is a formalization of 
the discussion of the introduction. 

Proposition 7.3. Distribution annihilating constraints 

Given a set of one-forms u>i(q), i = 1,... ,k, there exist smooth, linearly 
independent vector fields gj{q),j = 1,..., n— k such that 0Ji{q) ■ gj{q) = 0 
for all i and j. 

Proof. The uifs form a codistribution of dimension k in R n . We can 
choose local coordinates such that the set of one-forms is given by 


LVi — [0 * 1 ‘ 0 ^i,k -(-1 ' ' ' •> 


where the 1 in the preceding equation is in the ith entry, and the functions 
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an : 1 ” 


R are smooth functions. Define 


a i ,(j+k) 


9j ■■= 


0 


where the 1 is in the j +kth entry. The gj ’s are linearly independent and 
annihilate the constraints since 


dd * gj — k) 0- 

This shows that u>i ■ gj = 0 for i = 1,..., k and j = 1,..., n — k. Q 

In the language of distributions and codistributions, the results of this 
proposition are expressed by defining the codistribution 

D = spanjwi,..., u>k\ 


and the distribution 


A = span{c/i,... ,g n -k] 


and stating that 


A = Q x . 


We say that the distribution A annihilates the codistribution SI. The 
control system associated with the distribution A is of the form 


q = 9i(q)ui H-b g n -k{q)u n -k , 


with the controls Ui to be freely specified. 

These results of this section can be used to determine if a set of 
Pfaffian constraints is holonomic: 


Proposition 7.4. Integrability of Pfaffian constraints 

A set of smooth Pfaffian constraints is integrable if and only if the distri¬ 
bution which annihilates the constraints is involutive. 


2.3 Nonlinear controllability 

In view of Proposition 7.3, which yields a set of vector fields orthogonal 
to a given set of one-forms, it is clear that the motion planning problem 
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is equivalent to steering a control system. Thus, we will now restrict our 
attention to control systems of the form 


E : 


q = gi(q)ui -\ -h g m (q)um 


q G R" 
uGU CR™ 


(7.10) 


This system is said to be drift-free , meaning to say that when the controls 
are set to zero the state of the system does not drift. We assume that 
the gj are smooth, linearly independent vector fields on R™ and that 
their flows are defined for all time (i.e., the gj are complete). We wish 
to determine conditions under which we can steer from q 0 G R ra to an 
arbitrary qf £ R" by appropriate choice of u(-). 

A system E is controllable if for any qo,qf G R ra there exists a T > 0 
and u: [0,T] —s- U such that E satisfies g(0) = q 0 and q(T) = qj. A 
system is said to be small-time locally controllable at q o if we can reach 
nearby points in arbitrarily small amounts of time and stay near to qo at 
all times. Given an open set V C R n , define 7Z) (qo,T) to be the set of 
states q such that there exists u: [0, T] —> U that steers E from 3 ( 0 ) = qo 
to q(T) = qf and satisfies q(t) G V for 0 < t < T. We also define 

H v (q 0 ,<T)= |J 1l v (q 0 , t) 

0 <t<T 

to be the set of states reachable up to time T. A system is small-time lo¬ 
cally controllable ( locally controllable for brevity) if 7Z) (go, <T) contains 
a neighborhood of qo for all neighborhoods V of qo and T > 0. 

Let A = ... , g m ) be the Lie algebra generated by g±,... ,g m . It 

is referred to as the the controllability Lie algebra. From the construction 
involved in the definition of the Lie bracket in the previous subsection, 
we saw that by using an input sequence of 


Ui = +1 

u\ = 0 

ui = —1 
ui = 0 


u 2 = 0 for 0 < t < e 

u 2 = +1 for e < t < 2e 

u 2 = 0 for 2e < t < 3e 

u 2 = —1 for 3 e <t < 4e, 


we get motion in the direction of the Lie bracket [ 31 , 52 ]- If we were to 
iterate on this sequence, it should be possible to generate motion along 
directions given by all the other Lie products associated with the 3 ^. 
Thus, it is not surprising that it is possible to steer the system along all 
of the directions represented in £( 31 ,..., g m ). This is made precise by 
the following theorem, which was originally proved by W.-L. Chow (in 
somewhat different form) in the 1940s. 

Theorem 7.5 (Chow). The control system (7.10) is locally controllable 
at q G R ra if A q = T q R n . 
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qo 



Figure 7.2: Proof of local controllability. At each step we can find a 
vector field which is not in N k . 


This result asserts that the drift-free system E is controllable if the 
rank of the controllability Lie algebra is n. The condition of Chow’s 
theorem consists of checking the rank of the controllability Lie algebra 
and is hence referred to as the controllability rank condition. 

To prove Chow’s theorem, we prove the following pair of implications 
for a given system E in a neighborhood of a point q: 

Aq=T q W n => int77 A (q, <T)^{} •£=>• E is locally controllable, 

where A = £(gi, ■ ■ ■, g m ) and {} stands for the empty set. 

Proposition 7.6. Controllability rank condition 

If A q = for all q in some neighborhood of qo, then for any T > 0 

and neighborhood V of qo, int7£ v ( qo,<T ) is non-empty. 

Proof. The proof is by recursion. Choose /i € A. For ei > 0 sufficiently 
small, 

N i = {<Ptl(Qo) : 0 < ti < ei} 

is a smooth surface (manifold) of dimension one which contains points 
arbitrarily close to qo- Without loss of generality, take N\ C V. Assume 
N k Ckisa fc-dimensional manifold. If k < n, there exists q £ N k and 
fk +i G A such that fk+i ^ T q N k . If this were not so then A q C T q N k for 
any q in some open set W C N k . which would imply A\w C TN k . This 
cannot be true since dim A q = n > dim N k . For efc+i sufficiently small 

Nk+1 = {<Ptkti ° ‘'' ° : 0 < h < e i, * = 1, • ’ ‘ > k + 1} 

is a k + 1 dimensional manifold. Since e can be made arbitrarily small, 
we can assume N k+ 1 C V. 

If k = n, N k C V is an n-dimensional manifold and by construction 
N k C 1Z V (qo, <ei + ■ • • + e n ). Hence 7 Z l (qo, e) contains an open set. By 
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qo 



qi ewc n v ( qo ) 


Figure 7.3: Proof of local controllability. To show TZ V (qo) contains a 
neighborhood of the origin, we move to any point qj and map a neigh¬ 
borhood of qf to a neighborhood of qo by reversing our original path. 


restricting each ei < T/n, we can find such an open set for any T > 0. 
This proof is illustrated in Figure 7.2. □ 

Having established conditions under which the set int P v (q, < T) is 
not empty, we would like to determine if the set can be chosen so as to 
have go hr its interior. This is the subject of the next proposition: 

Proposition 7.7. Local controllability 

The interior of the set 1Z V (go, <T) is non-empty for all neighborhoods V 
of qo and T > 0 if and only ifT, is locally controllable at qo. 

Proof. The sufficiency follows from the definition of locally controllable. 
To prove necessity, we need to show that 1Z V (go, < T) contains a neigh¬ 
borhood of g 0 . Choose a piecewise constant u: [0,T/2] —> U such that u 
steers g 0 to some qq £ 7 Z v (q 0 , <T/ 2) and q(t) £ V. Let (j)f be the flow 
corresponding to this input (as given in the proof of the previous theo¬ 
rem). Since S is drift-free, we can flow backwards from qf to go using 
u'{t) = — u(T/2 — f), t £ [0,T/2]. The flow corresponding to u' is 
By continuity of the flow, there exists W C 7 Z' 1 (go, T/2) such that g/ £ W 
and ((ff)~ 1 (W) C V for all t. Furthermore, (<i s a neighbor¬ 
hood of go- It follows that P) (go>< T) contains a neighborhood of go 
since we can concatenate the inputs which steer go to g/ £ W with u' to 
obtain an open set containing go- This is illustrated in Figure 7.3. P 

In principle, we now have a recipe for solving the motion planning 
problem for systems which meet the controllability rank condition. Given 
an initial point go and final point qq, find finitely many intermediate via 
points gi, g 2 ,..., g p £ K” and neighborhoods Vi such that 

U K Vi bi,<T) 

2=1 
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Figure 7.4: Steering between go and g/. 


contains the straight line segment connecting go to gy, as shown in Figure 
7.4. Then there exists a control law of p segments which steers from go 
to g/. The difficulty with this procedure and the preceding theorems in 
this section is that they are non-constructive. It is in principle possible 
to solve the motion planning problem for a given set of constraints of the 
form 

Wj(g)g = 0 i = l,...,k 

for arbitrary given q 0 and gy, provided that the associated control system 
q = gi(q)ui H-1- g n -k(q)u n - k 

has a full rank controllability Lie algebra. However, the preceding the¬ 
orems do not give a constructive procedure for generating paths for the 
system joining go and g/. This constructive controllability is the goal of 
the next chapter. 


3 Examples of Nonholonomic Systems 

We now present a set of examples of systems with nonholonomic con¬ 
straints which we will use repeatedly throughout this chapter and the 
next to illustrate the different concepts. Nonholonomic constraints arise 
in two kinds of situations: 

1. Bodies in contact with each other which roll without slipping 

2. Conservation of angular momentum in a multibody system 

An example of the first kind can be found in the problem of dextrous 
manipulation with a multifingered robot hand. Here the nonholonomic 
constraint arises from the fingers rolling without slipping on the surface 
of a grasped object. Other such examples arise in path planning problems 
for mobile robots or automobiles, where the wheels roll without slipping. 
For examples of the second kind, we have motion of a satellite with robotic 
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Figure 7.5: A simple hopping robot. 


appendages moving in space, where angular momentum is conserved, or 
a diver or gymnast in mid-air maneuvers. 

In the sequel, we will give a description of several nonholonomic sys¬ 
tems. The proof of their nonholonomy (that is the impossibility of finding 
functions of the configuration variables which are “integrals” of the con¬ 
straints) is deferred to Section 4. 

Example 7.2. Hopping robot in flight 

As our first example, we consider the dynamics of a hopping robot in 
the flight phase, as shown in Figure 7.5. This robot consists of a body 
with an actuated leg that can rotate and extend; the “constraint” on the 
system is conservation of angular momentum. 

The configuration q = (ip, l, 6) consists of the leg angle, the leg exten¬ 
sion, and the body angle of the robot. We denote the moment of inertia 
of the body by I and concentrate the mass of the leg, m, at the foot. 
The upper leg length is taken to be d , with l representing the extension 
of the leg past this point. The total angular momentum of the robot is 
given by 

I9 + m(l + d) 2 (9 + ip). (7.11) 

Assume that the angular momentum of the robot is initially zero. Equa¬ 
tion (7.11) is a single Pfaffian constraint in the three velocities ip, l, and 9. 
Thus, the associated control system has two inputs—three configuration 
variables minus one constraint. As a basis for the 2-dimensional right 
null space of the constraint, we choose one vector field corresponding to 
controlling the leg angle ip, and the other corresponding to controlling 
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Figure 7.6: A simplified model of a planar space robot. 


the leg extension l; i.e., set ip = U\ and l = m 2 . Then, we have 


1 


[01 

0 

52(g) = 

1 

m(l-\-d) 2 

I+m{l+d) 2 

0 


and the equivalent control system is given by 

q = gi{q)ui +g2{q)u 2 - 

Example 7.3. Planar space robot 

Figure 7.6 shows a simplified model of a planar robot consisting of two 
arms connected to a central body via revolute joints. If the robot is free 
floating, then the law of conservation of angular momentum implies that 
moving the arms causes the central body to rotate. In the case that 
the angular momentum is zero, this conservation law can be viewed as a 
Pfaffian constraint on the system. 

Let M and I represent the mass and inertia of the central body and 
let m represent the mass of the arms, which we take to be concentrated 
at the tips. The revolute joints are located a distance r from the middle 
of the central body and the links attached to these joints have length 
l. We let (xi,yi) and (x 2 ,y 2 ) represent the position of the ends of each 
of the arms (in terms of 0, ipi, and ip 2 )- Assuming that the body is 
free floating in space and that friction is negligible, we can derive the 
constraints arising from conservation of angular momentum. 
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Let 0 be the angle of the central body with respect to the horizontal, 
ipi and ip 2 the angles of the left arm and right arms with respect to the 
central body, and peM 2 the location of a point on the central body (say 
the center of mass). The kinetic energy of the system has the form 


K=l(M + 2ro)||p|| 2 + he 2 + lm{x\ + yf) + \m{x 2 2 + y\) 


= -(M + 2m)\\p\\ 2 


~1pl 

T r 

lf>2 


9 _ 



a 11 ai 2 ai3 

<212 a 22 <223 

<213 <223 033 



W 


^2 


9 _ 


where a i: j can be calculated as 

an = 022 = ml 2 
012 = 0 

013 = ml 2 + mr cos ipi 
023 = ml 2 + mr cos %l) 2 

033 = I + 2ml 2 + 2 mr 2 + 2 mrl cosipi + 2 mrl cos ip 2 . 


Note that the kinetic energy of the system is independent of the variable 
9. It therefore follows from Lagrange’s equations that in the absence of 
external forces, 

d^3L _ dL 
dt 39 dO 

Thus the quantity is a constant of the motion. This is precisely the 
angular momentum, pt, of the system: 

dL • • 

d = —x = a 13 i/>i + 023^2 + a 33 9. 
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If the initial angular momentum is zero, then conservation of angular 
momentum ensures that the angular momentum stays zero, giving the 
following constraint equation 


ai3('0)V’i + a 23 (ip)ip 2 + a 33 (ilj)d = 0. (7-12) 


Since the variables that are actuated are the hinge angles of the left 
and right arm, we choose as inputs u\ = ipi and u 2 = ip 2 - Using these in 
equation (7.12) and setting q = (^ 1 ,^ 2 ,^), we get 

<7 = 9i(q)ui +g 2 (q)u 2 


where 


1 


0 

0 

32 (g) = 

1 

— CL 13 


— a 23 

L 033 J 


L «33 J 
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Figure 7.7: Disk rolling on a plane. 


Example 7.4. Disk rolling on a plane 

Consider the motion of a thin flat disk rolling on a plane shown in Fig¬ 
ure 7.7. The configuration space of the system is parameterized by the 
xy location of the contact point of the disk with the plane, the angle 9 
that the disk makes with the horizontal line, and the angle (j> of a fixed 
line on the disk with respect to the vertical axis. We assume that the 
disk rolls without slipping. As a consequence we have that 

x — p cos 9 <j) = 0 
y — psin 9 <j> = 0, 


where p > 0 is the radius of the disk. Writing these equations in the form 
of Pfaffian constraints with q = [x, y, 9 , </>) we have 


10 0 —pcos9 

0 10 — psin# 


9 = 0. 


Choosing 9 = u±, the rate of rolling, and cf> = U 2 , the rate of turning 
about the vertical axis, we have the associated control system: 


pcos9 


'O' 

p sin 9 

Ml + 

0 

0 

1 

1 


0 


(7.13) 


Example 7.5. Kinematic car 

Consider a simple kinematic model for an automobile with front and rear 
tires, as shown in Figure 7.8. The rear tires are aligned with the car, while 
the front tires are allowed to spin about the vertical axes. To simplify the 
derivation, we model the front and rear pairs of wheels as single wheels 
at the midpoints of the axles. The constraints on the system arise by 
allowing the wheels to roll and spin, but not slip. 

Let ( x,y,9,(f >) denote the configuration of the car, parameterized by 
the xy location of the rear wheel(s), the angle of the car body with 
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Figure 7.8: Kinematic model of an automobile. 


respect to the horizontal, 6 , and the steering angle with respect to the 
car body, (f>. The constraints for the front and rear wheels are formed 
by setting the sideways velocity of the wheels to zero. In particular, the 
velocity of the back wheels perpendicular to their direction is sin(9i — 
cos Oy and the velocity of the front wheels perpendicular to the direction 
they are pointing is sin(6+4>)x — cos(0+<f>)y — W cos <f>, so that the Pfaffian 
constraints on the automobile are: 

sin(0 + (j))x — cos (9 + (f>)y — l cos (j>9 = 0 
sinfli; — cos 6y = 0. 

Converting this to a control system with the inputs chosen as the 
driving velocity u\ and the steering velocity w 2 gives 


X 


cosf? 


'O' 

y 


sin(9 

U\ + 

0 

0 

— 

j tan (j> 

0 

A 


0 


1 


it 2 . 


(7.14) 


For this choice of vector fields, u\ corresponds to the forward velocity of 
the rear wheels of the car and u 2 corresponds to the velocity of the angle 
of the steering wheel. 

Example 7.6. Fingertip rolling on an object 

Let us analyze the motion of a curved fingertip over a curved object. As 
we discussed in Section 6 of Chapter 5, we parameterize the object surface 
by a 0 £ R 2 , the fingertip surface by a/ € R 2 , and the angle of contact 
by "0 £ S 1 , giving a 5-dimensional configuration space. The kinematic 
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equations of contact are given by 


a f =Mj\K f + k 0 )- 1 

a 0 = M- 1 R i ,(K f + K 0 )~ 1 
ip = u z + TfMfdtf + T 0 M 0 


- K n 


+ K f 


(7.15) 


The rolling constraint is obtained by setting the sliding velocity and the 
velocity of rotation about the contact normal to zero: 


x =0 to z = 0. (7.16) 

Substituting (7.16) into equation (7.15) yields the following constraints: 


Mfdif — Rj/,M 0 a 0 = 0 

TfMfdtf + T 0 M 0 a 0 — ip = 0. 


(7.17) 


If we set q = {otf,a 0 ,ip) € R 5 , then the foregoing set of three constraints 
is of the form 

tOi{q)q = 0 i = 1,2, 3. 

To obtain a control system associated with these constraints, we let u\ = 
u> x and U 2 = oj v in the kinematic equations for rolling contact. After 
rearranging the results, we have 


9 = 


M 7 

M~ 1 Rj /j 

Tf + T a R^_ 


(■Kf + K 0 y 


u 1 


-1 

0 


U 2 


(7.18) 


We now specialize the example to the case that the object is flat 
and the fingertip is a sphere of radius one. The curvature forms, metric 
tensors, and torsions for the fingertip and the object have been derived 
in Example 5.7 and are reproduced here for convenience: 


M 0 = 
T 0 = 


'0 

o' 

K f = 

0 

0 

'1 

o' 

Mf = 

0 

1 

[0 

0 ] 

Tf= [ 


0 

1 

0 

COS qi 
— tangi] . 


Substituting the above results into (7.17) gives 


1 0 
0 cos q\ 
0 sin qi 


— cos <75 sin <75 

sin <75 cos <75 

0 0 


0 

0 

1 


<7 = 0. 
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In this case, the formula (7.18) gives, with the inputs being the rates of 
rolling about the two tangential directions, 


qi 


0 


-1 

92 


sec qi 


0 

93 

= 

- sin q 5 

Ui + 

— COS (?5 

94 


— COS (75 


sin (75 

. 95 . 


— tan qi 


0 


4 Structure of Nonholonomic Systems 

We return to the problem of motion planning for systems satisfying linear 
velocity constraints of the form 

Vi{q)q = 0 i = 

In Section 2 we showed how the problem of finding feasible trajectories 
in the configuration space could be dualized to one of finding trajectories 
of the control system 


q = 9 i{q)ui-\ - \-g m (q)u m , (7.20) 

with m = n — k and u>i(q)gj(q) = 0. From the controllability rank 
condition, it follows that one can find a trajectory joining an arbitrary 
starting point and end point if the rank of the Lie algebra generated by 
gi,... ,g m is n. If A q y and in addition A g has a constant rank 

n — p which is less than n, then it follows from Frobenius’ theorem that 
there exist functions hi(q) = Ci, i = 1 ,... ,p such that 

Ui{q)q = 0 i = l,...,k <*=> hj(q) = Cj j = l,...,p. 

Consider this a little further: since the dimension of A is greater than 
or equal to the dimension of A, it follows that p < k. Thus, the num¬ 
ber of functions whose level sets are tangential to the given distribution 
are fewer than the dimension of the distribution. The process of con¬ 
verting from the given constraints, specified as a codistribution, to an 
equivalent control system and then integrating the involutive closure of 
this distribution may seem to be convoluted. It is indeed possible to deal 
directly with a given codistribution and to find the maximal integrable 
codistribution contained within it, but this involves methods of exterior 
differential systems which are beyond the scope of this book. Of course, 
in the event that A = for all q, then p = 0, i.e., there are no non¬ 

trivial functions which integrate the given constraints. In this case the 
distribution is said to be completely nonholonomic, as was noted earlier. 

In this section, we will try to make precise some notation that we will 
use in dealing with nonholonomic systems and apply it to the examples 
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that we considered in Section 3. Some additional machinery to study the 
growth of the controllability Lie algebra is discussed at the end of this 
section. 


4.1 Classification of nonholonomic distributions 


The complexity of the motion planning problem is related to the or¬ 
der of Lie brackets in its controllability Lie algebra. Here we develop 
some concepts which allow us to classify nonholonomic systems. Let 
A = span{gi,..., g m } be the distribution associated with the control 
system (7.20). Define Ai = A and 

Aj = Aj_i + [Ai, Aj_i], 

where 

[Ar, Ai_!] = span{[g, h]:g€A u h€ Aj_ x }. 

It is clear that Aj C Aj+i. The chain of the distributions Aj is defined 
as the filtration associated with the distribution A = A^ Each Aj is 
defined to be spanned by the input vector fields plus the vector fields 
formed by taking up to * — 1 Lie brackets of the generators, i.e., elements 
of Ai. The Jacobi identity (see Proposition 7.1, page 325) implies that 
[Ai.Aj-] C [Ai, Aj + j_i] C Aj_| ,j. The proof of this fact is left as an 
exercise. 

A filtration is said to be regular in a neighborhood U of qo if 


rank Aj(g) = rank Aj(go) Vg £ U. 


We say the control system (7.20) is regular if the corresponding filtration 
is regular. If a filtration is regular, then at each step of its construction, Aj 
either gains dimension or Aj + i = Aj, so that the construction terminates. 
If rank A, + j = rank Aj, then Aj is involutive and hence Aj+j = Aj for 
all j > 0. Clearly, rank Aj < n and hence if a filtration is regular, then 
there exists an integer n < n such that Aj = A re for all i > k. 


Definition 7.2. Degree of nonholonomy 

Consider a regular filtration {Aj} associated with a distribution A. The 
smallest integer k such that the rank of A K is equal to that of A k+ i is 
called the degree of nonholonomy of the distribution. 


We know that rank A re < n. In general, let the rank of A K = n — p. 
Then, by Frobenius’ theorem there are p functions hi for i = 1 
whose level surfaces are the integral manifolds of A K . Thus, the state q 
of the control system must be confined to the a level set of the hfs. This, 
then, is the complete answer to the question we posed ourselves at the 
beginning of this chapter. The maximum number of functions hi such 
that 


S Pan{ ~fjq I C spanjwi,..., u k ) 
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is given to be the set of functions such that 


rdhx 

s pan l — 


dhp 
dq 1 


(A) x 


by Frobenius’ theorem. If p = 0, that is rank A K is equal to n, then 
there are no nontrivial functions hi and it is possible to steer between 
arbitrary given initial and final points. This is Chow’s theorem, which 
was discussed in the previous section. Chow’s theorem is actually also 
valid when the filtration Aj is not regular, as long as A is smooth and 
constant rank. 

We now give a definition which serves to classify the growth of a 
filtration: 


Definition 7.3. Growth vector, relative growth vector 

Consider a regular filtration associated with a given distribution A and 
having degree of nonholonomy re. For such a system, we define the growth 
vector r £ as 

ri = rank A*. 

We define the relative growth vector a £ Z K as cq = r^ — r^_i and r 0 := 0. 

The growth vector for a regular filtration is a convenient way to rep¬ 
resent complexity information about the associated controllability Lie 
algebra. 


4.2 Examples of nonholonomic systems, continued 

In this subsection, we illustrate the classification of nonholonomic systems 
on the examples that were developed in Section 3. 

Example 7.7. Hopping robot in flight 

Recall from Section 3 that the configuration space for the hopping robot 
in flight is given by {ip, l, 0): the leg angle, leg extension, and body angle 
of the robot. Since we control the leg angle and extension directly, we 
choose their velocities as our inputs and the control system associated 
with the hopper is given by 


ip = U\ 

l = U 2 


m{l + d) 2 
I + il + d) 2 


u 1 - 


The controllability Lie algebra is given by 


1 


r°i 


r o ] 

0 

92 = 

i 

93 = [gi,92} = 

0 

m(l-\-d ) 2 

0 

2 lm(l-\-d) 

I+{l+d) 2 \ 



L(I+m(l+d) 2 ) 2 J 
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In a neighborhood of l = 0, spanjgi, < 72 , 53 } is full rank and hence the 
hopping robot has degree of nonholonomy 2 with growth vector (2,3) and 
relative growth vector (2,1). 

Example 7.8. Planar space robot 

From Example 7.3, we have that the angular momentum conservation 
constraint yields 

ai3(V>)0i + a23(V0^2 + a 3 3(t/>)0 = 0, 

where the vector of configuration variables is q = (^i,^2,0)- Using the 
control equations derived in Example 7.3, we have 

1 

9i= 2 0 

_ ml -\-mr cos i/>i 

I-\-2ml 2 -\-2mr 2 -\-2mrl cos ipi-\-2mrl cos 02 - 


9 2 


1 

0 

ml 2 -\-mr cos 02 

I-\-2ml 2 -\-2mr 2 -\-2mrl cos 0i+2mrZ cos 02 - 


and the Lie bracket is 


93 — [31 1 92} 


0 

0 

2ra 2 Z 2 r(—Z sin 0i—r sin(0i —02)+Z sin 0 2 ) 
(J+2mZ 2 +2mr 2 +2raZr cos '0i+2mZr cos t/^) 2 


The vector field <73 is zero when ipi = ip 2 and hence the filtration {A;} 
is not regular. By computing higher order Lie brackets, however, it is 
possible to show that A q = T 9 K 3 in a neighborhood of q = 0 and the 
system is controllable. 

Example 7.9. Disk rolling on a plane 

From Example 7.4, the control system which describes a disk rolling on 
a plane is described by the distribution spanned by 



pcosO 


0 " 


psmO 


0 

9i = 

0 

92 = 

1 


1 


0 


The control Lie algebra is constructed by computing the following vector 
fields: 



p sin 9 


pcos$ 

93 = [31 > 32 ] = 

—p cos 3 
0 

34 = [ 32 , 33 ] = 

psin3 

0 


0 


0 
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For all q , span{< 7 i, < 72 , 53,54} is full rank and hence the rolling disk has 
degree of nonholonomy 3 with growth vector (2,3,4). The relative growth 
vector for this system is ( 2 , 1 , 1 ). 

Example 7.10. Kinematic car 

Recall that (x, y , 9 , (j>) denotes the configuration of the car, parameterized 
by the location of the rear wheel(s), the angle of the car body with respect 
to the horizontal, and the steering angle with respect to the car body. 
The constraints for the front and rear wheels to roll without slipping are 
given by the following equations: 

sin(5 + (j))x — cos (9 + <f>)y — l cos <f> 9 = 0 
sinfli; — cos 9y = 0 . 

Converting this to a control system with the driving and steering velocity 
as inputs gives the control system of equation (7.14). 

To calculate the growth vector, we build the filtration 



cos 9 


O' 


sin 9 


0 

5i = 

j tan (j) 

52 = 

0 


0 


1 


93 = [ 51 , 52 ] 


0 

0 


r sin 6 


l COS 2 4> 

cos 6 

1 

54 = [51,53] = 

COS 2 (f) 

0 

l COS 2 4> 


0 


0 


The vector fields { 51 , 52,53,54} are linearly independent when <f> ^ ±7r. 
Thus the system has degree of nonholonomy 3 with growth vector r = 
(2,3,4) and relative growth vector a = (2,1,1). The system is regular 
away from (j) = ±7r/2, at which point g\ is undefined. 


Example 7.11. Spherical finger rolling on a plane 

Let the inputs be the two components of rolling velocities, i.e., Ui = u> x 
and U 2 = oj y . The associated control system is derived in (7.19), which 
in vector field form reads 


0 


-1 

sec < 7 i 


0 

- sin q 5 

52 = 

— COS <75 

— cos <75 


sin q 5 

— tan 


0 
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Constructing the filtration, we have 


0 


0 

tan 771 sec q± 


0 

— tan qi sin q 5 

54 = [ 51 , 53 ] = 

— COS (75 

— tan <71 cos <75 


sin <75 

— sec 2 <71 


0 


95 = [ 52 , 53 ] 


0 

— (1 + sin 2 qi) sec 3 <71 
2 sin q 5 sec 2 qi 
2 cos <75 sec 2 <71 
2 tan < 7 i sec 2 q\ 


In a neighborhood of q = 0 (more specifically in a neighborhood not 
containing 51 = f) the vector fields { 51 , 52 , 53 , 54 , 55 } are linearly inde¬ 
pendent, thus establishing that the degree of nonholonomy is 3 and that 
the growth vector is (2,3,5). The relative growth vector is (2,1, 2). 


4.3 Philip Hall basis 

Let £(51,... ,5 m ) be the Lie algebra generated by a set of vector fields 
51, ..., 77m• One approach to equipping £(51, ..., g m ) with a basis is to 
list all the generators and all of their Lie products. The problem is that 
not all Lie products are linearly independent because of skew-symmetry 
and the Jacobi identity. The Philip Hall basis is a particular way to select 
a basis which takes into account skew-symmetry and the Jacobi identity. 

Given a set of generators { 771 , • • • , 5 m}, we define the length of a Lie 
product recursively as 


Z(fli) = 1 i = 1,- • • ,m 

l([A,B}) = 1(A) + 1(B), 

where A and B are themselves Lie products. Alternatively, 1(A) is the 
number of generators in the expansion for A. A Lie algebra is nilpotent 
if there exists an integer k such that all Lie products of length greater 
than k are zero. The integer k is called the order of nilpotency. 

A Philip Hall basis is an ordered set of Lie products H = {Bi} satis¬ 
fying: 

1 . 77 ^ £ H 1 i = 1 ,..., m 

2. If l(Bi) < l(Bj) then Bi < Bj 

3. [Bi,Bj\ £ H if and only if 

(a) Bi,Bj £ H and Bi < Bj and 
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(b) either Bj = gk for some k or Bj = [Bi,B r ] with Bi,B r £ H 
and Bi < Bi. 

The proof that a Philip Hall basis is indeed a basis for the Lie algebra 
generated by {gy,... ,g m } is beyond the scope of this book and may be 
found in [38] and [104]. A Philip Hall basis which is nilpotent of order 
k can be constructed from a set of generators using this definition. The 
simplest approach is to construct all possible Lie products with length 
less than k and use the definition to eliminate elements which fail to 
satisfy one of the properties. In practice, the basis can be built in such a 
way that only condition 3 need be checked. 

Example 7.12. Philip Hall basis of order 3 

A basis for the nilpotent Lie algebra of order 3 generated by gi, 32,53 is 

3i 92 33 

[ 31 , 32 ] [ 32 , 33 ] [33,3i] 

[3i, [ 31 , 32 ]] [ 31 , [31,33]] [ 32 , [ 31 , 32 ]] [ 32 , [ 31 , 33 ]] 

[ 32 , [ 32 , 33 ]] [33, [ 31 , 32 ]] [33, [31,33]] [33, [ 32 , 33 ]] 

Note that [ 31 , [ 32 , 33 ]] does not appear since 

[3i, [ 32 , 33 ]] + [ 32 , [33,3i]] + [33, [ 31 , 32 ]] = 0 

by the Jacobi identity and the second two terms in the formula are already 
present. 
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5 Summary 

The following are the key concepts covered in this chapter: 

1. Nonholonomic constraints are linear velocity constraints of the form 

Ui(q)q = 0 i = 1,..., k 

which cannot be integrated to give constraints on the configuration 
variables q alone. By choosing gj(q),j = 1 ,,n — k =: m to be 
a basis for the null space of the linear velocity constraints, we get 
the associated control system 

q = gi{q)ui H-b g m (q)um- 

The problem of nonholonomic motion planning consists of finding 
a trajectory g(-) : [0, T] —> M”, given q( 0) = go and q{T) = qf. 

2. The Lie bracket between two vector fields / and g on R" is a new 
vector field [/, g] defined by 

[= ^/(<?) - 

3. A distribution A is a smooth assignment of a subspace of the tangent 
space to each point q € R n . One important way of generating it is 
as the span of a number of vector fields: 

A = span{gi,.. .,g m }. 

The distribution A is said to be regular if the dimension of A q does 
not vary with q. The distribution A is said to be involutive if it 
is closed under the Lie bracket, that is if for all /, g £ A, we have 
[f,g] e A. 

4. A distribution A of dimension k is said to be integrable if there 
exist n — k independent functions whose differentials annihilate the 
distribution. Frobenius’ theorem asserts that a regular distribution 
is integrable if and only if it is involutive. A Pfafhan system or 
codistribution Q 

f l = span{wi,..., u>k} 

is completely nonholonomic if the involutive closure of the distribu¬ 
tion A = fl- 1 spans 1" for all q. 

5. Consider the system 

q = gi(q)ui H- ^ g m (q)u m - 
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The controllability Lie algebra is the Lie algebra generated by the 
vector fields gi,... ,g m . It is the smallest Lie algebra containing 
gi,...,g m . Chow’s theorem asserts that if the controllability Lie 
algebra is full rank, we can steer this system from any initial to any 
final point. 

6. Given a distribution A, the filtration associated with A is defined 
by Ai = A and 

A i = Aj_i + [Ai, Aj_i], 

where 

[Ai, Aj_i] = span{[g, h) : g € A 1 , h e Aj_i}. 

The filtration is said to be regular if each of the A, are regular. For 
a regular filtration, the smallest integer n at which rank A K is equal 
to that of A k _|_!, A k+2 , ... is called the degree of nonholonomy of 
the distribution. The growth vector r £ for a regular filtration 
is defined as ri := rank Aj. The relative growth vector a £ Z K is 
defined as cq = Ty — rj_i with vq = 0. 

7. Given A = spanj^i,..., g m }, a Lie product is any nested set of Lie 
brackets of the generators gi. A Lie algebra generated by A is said 
to be nilpotent if there exists an integer k such that all Lie products 
of length greater than k are zero. A Philip Hall basis is an ordered 
set of Lie products chosen by a set of rules so as to keep track of the 
restrictions imposed by the properties of the Lie bracket, namely 
skew-symmetry and the Jacobi identity. 
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Vershik and Gershkovic [117]. The notation we follow is theirs and is 
presented in [78]. 
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A somewhat less obvious application of the methods of this chapter is 
in the analysis of control algorithms for redundant manipulators. In this 
application, one looks for an algorithm such that closed trajectories of the 
end-effector generate closed paths in the joint space of the manipulator. 
This is closely related to the integrability of a set of constraints. A good 
description of this is in the work of Shamir and Yomdin [105], Baillieul 
and Martin [5], Chiacchio and Siciliano [17], and De Luca and Oriolo [23]. 
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Exercises 


1. Show that the controllability rank condition is also a necessary con¬ 
dition for local controllability under the usual smoothness and reg¬ 
ularity assumptions. 

2. Show that the differential constraint in R 5 given by 

[0 1 p sin q 5 p cos q% cos q$\q = 0 

is nonholonomic. 

3. Use the definition of the Lie bracket to prove the properties listed 
in Proposition 7.1. 

4. Consider the system E, 

q = gi(q)ui H- \~ g m (q)u m . 

Let u : [0,T] —> R m be input which steers S from go to g/ in T 
units of time. 

(a) Show that the input u : [0,1] —► R m defined by 

u(t) = u(t/T) 

steers a from g 0 to gy in 1 unit of time. 

(b) Show that the input u : [0,1] —* R m defined by 

u(t) = —12(1 — t) 
steers a from qf to g 0 in 1 unit of time. 

5. Spheres rolling on spheres 

Derive the control equation for a unit sphere in rolling contact with 
another sphere of radius p with the same inputs as in Example 7.6. 
Show that the system is controllable if and only if p ^ 1. 

6. Car with N trailers 

The figure below shows a car with N trailers attached. We attach 
the hitch of each trailer to the center of the rear axle of the previous 
trailer. The wheels of the individual trailers are aligned with the 
body of the trailer. The constraints are again based on allowing 
the wheels only to roll and spin, but not slip. The dimension of the 
state space is IV + 4 with 2 controls. 
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Parameterize the configuration by the states of the automobile plus 
the angle of each of the trailers with respect to the horizontal. Show 
that the control equation for the system has the form 


x = COS 9q III 
y = sin 6>o u\ 

<j> = u 2 

0 o = 7 tan 4> ui 



7. Firetruck 

A firetruck can be modeled as a car with one trailer, with the dif¬ 
ference that the trailer is steerable, as shown in the figure below. 



y 


- x 


The constraints on the system are similar to that of the car in 
Section 3, with the difference that back wheels are steerable. Derive 
the nonlinear control system for a firetruck corresponding to the 
control inputs for driving the cab and steering both the cab and 
the trailer, and show that it represents a controllable system. 
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8. Prove that a 1-dimensional distribution A q = span{/(g)} is invo- 
lutive. More specifically, show that for any two smooth functions a 
and j3 

[«/,/?/] e A. 


9. 


Prove that the two definitions of Lie bracket given in this chapter, 
namely, 


[/.fl] 


dg f _9f 

dq 1 dq g 


and 

L[f,g] a = Lf(L g a) — L g (Lfa) Va : R" —> R, 


are equivalent. 


10. Use induction and Jacobi’s identity to prove that 


[A», Aj] c [Ai, Aj_|_j_i] c Aj-y, 

where A = Ai C A 2 C ■ ■ ■ is a filtration associated with a distri¬ 
bution A. 


11. Let Aj, i = 1,..., k be a regular filtration associated with a distri¬ 
bution. Show that if rank(A; + i) = rank(A;) then Aj is involutive. 
(Hint: use Exercise 10). 

12. Satellite with 2 rotors 

Figure 7.9 shows a model of a satellite body with two symmetrically 
attached rotors, where the rotors’ axes of rotation intersect at a 
point. The constraint on the system is conservation of angular 
momentum. 

(a) Assuming that the initial angular momentum of the system is 
zero, show that the (body) angular velocity, u >\, of the satellite 
body is related to the rotor velocities ( 1 / 1 , 112 ) by 

loi = bim + b 2 U2 (7-21) 

where bi,b 2 £ R 3 are constant vectors. 

Equation (7.21) gives rise to a differential equation in the ro¬ 
tation group 50(3) for the satellite body 

R{t) = R{t)(b 1 u 1 + b 2 u 2 ). (7.22) 

(b) Obtain a local coordinate description of (7.22) using the Eu¬ 
ler parameters of 50(3) (from Chapter 2) and show that the 
resulting system is controllable. 
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rotor 



Inertia frame 



Figure 7.9: A model of a satellite body with two rotors. The satellite 
can be repositioned by controlling the rotor velocities. (Figure courtesy 
of Greg Walsh) 


13. The figure below shows a simplified model of a falling cat. It consists 
of two pendulums coupled by a spherical joint. The configuration 
space of the system is Q = § 2 x § 2 , where 8 2 is the unit sphere in 
R 3 . 



(a) Derive the PfafBan constraints arising from conservation of an¬ 
gular momentum and dualize the results to obtain the control 
system for nonholonomic motion planning. 

(b) Is the system in part (a) controllable? 

14. Write a computer program to write a Philip Hall basis of given order 
for a set of m generators g ±,..., g m . Use your program to generate 
a Philip Hall basis of order 5 for a system with 2 generators. 
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15. Consider the system of Exercise 6. Write a computer program (or 
use Mathematica or any other symbolic manipulation software pack¬ 
ages) to compute the filtration associated with the system. Show 
that the system is controllable, with degree of nonholonomy N + 2 
and relative growth vector (2,1,..., 1). 

16. In this chapter, we restricted ourselves to constraints of the form 


LOi(q)q = 0 i = l,...,k. 


Consider what would happen if the constraints were of the form 


u>i(q)q = Ci i = l,...,k 


for constants Cj. Dualize these constraints to get an associated 
control system of the form 


n—k 



What is the formula for /(g)? Apply this method to the space 
robot with nonzero angular momentum. What difficulties would 
one encounter in path planning for these examples? These systems 
are called systems with drift. 

17. (Hard) Show that for a regular system the growth vector is bounded 
above by 

01; = - ( (oh) 1 - ) * > !» 

where er, is the maximum relative growth at the ith stage and j\i 
means all integers j such that j divides i. If a % = o,j for all i, we 
say that the system has maximum growth. 
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Chapter 8 

Nonholonomic Motion 
Planning 


This chapter provides a survey of some of the techniques which have 
been used in planning paths for nonholonomic systems. This is an area 
of active research; here, we only provide an introduction to some of the 
recent techniques that may be applied. In addition to planning paths 
satisfying the constraints of the form of 

u>i{q)q = 0 i = l,...,k, 

one frequently has to make sure that the trajectory q(-) does not intersect 
obstacles (which are modeled as infeasible regions in the state space). For 
example, in the case of fingers rolling on the surface of a grasped object, 
obstacle avoidance may consist of keeping the fingers from colliding with 
each other or with the grasped object. Conventional (holonomic) path 
planners implicitly assume that arbitrary motion in the configuration 
space is allowed as long as obstacles are avoided. If a system contains 
nonholonomic constraints, many of these path planners cannot be directly 
applied since they generate paths which violate the constraints. For this 
reason, it is important to understand how to efficiently compute paths 
for nonholonomic systems. 


1 Introduction 

In this section, we introduce the reader to some of the general approaches 
to nonholonomic motion planning and give an outline of the chapter. 
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Optimal control 

Perhaps the most well-formulated method for finding trajectories of a 
general control system is the use of optimal control. By attaching a 
cost functional to each trajectory, we can limit our search to trajectories 
which minimize a cost function. Typical cost functions might be the 
length of the path (in some appropriate metric), the control cost, or the 
time required to execute the trajectory. 

If the system has bounds on the magnitudes of the inputs, it makes 
sense to solve the motion planning problem in minimum time. It is well 
known that for many problems, when the set of allowable inputs is con¬ 
vex, then the time-optimal paths consist of saturating the inputs at all 
times (this is often referred to as bang-bang control). The inputs may 
change between one set of values and another at a possibly infinite num¬ 
ber of switching times. Choosing an optimal trajectory is then equivalent 
to choosing the switching times and the values of the inputs between 
switching times. 

Piecewise constant inputs 

Related to the bang-bang trajectories of optimal control, it is also possible 
to steer nonholonomic systems using piecewise constant inputs. Perhaps 
the most naive way of using constant inputs is to pick a time interval and 
generate a graph by applying all possible sequences of inputs (discretized 
to finitely many values). Each node on the graph corresponds to a con¬ 
figuration, and branches indicate the choice of a fixed control over the 
time interval. The size of the graph grows as m d , where m is the number 
of input combinations considered at each step and d is the number of 
steps. Since we do not know how long to search, the amount of computer 
memory required by such an algorithm can be very large. Also, we are 
likely not to hit our goal exactly, so some post-processing must be done 
if exact maneuvering is needed. 

Recently, a very elegant and general motion planning method us¬ 
ing piecewise constant inputs has also been developed by Lafferriere and 
Sussmann [54]. They consider the case of a nilpotent system. Recall 
from Chapter 7 that a distribution A spanned by < 71 ,, g m is nilpotent 
of order k if all the Lie products with more than k terms vanish. The 
advantage of nilpotent Lie distributions is that certain computations are 
greatly simplified, as we shall see in Section 3. The main tool in their 
method is the Baker-Campbell-Hausdorff formula. If the system is not 
nilpotent, it can be shown that if the initial and final points are close, 
then the algorithm of Lafferriere and Sussmann moves the original sys¬ 
tem closer to the goal by at least half. By breaking the path up into 
small pieces, we can move arbitrarily close to the goal with repeated 
applications of the algorithm. 
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Canonical paths 


A third approach to solving the nonholonomic path planning problem 
is by choosing a family of paths which can be used to produce desired 
motions. For example, we might consider paths for a car that cause a 
net rotation of any angle, or a translation in the direction that the car 
is facing. We can then move to any configuration by reorienting the car, 
driving forward, and again reorienting the car. The path used to cause 
a net rotation might consist of a set of parameterized piecewise constant 
inputs or a heuristic trajectory. The set of canonical paths used for a 
given problem is usually specific to that problem. In some cases the 
paths may be derived from some unifying principle. For example, if we 
could solve the optimal control problem in closed form, these optimal 
paths would form a set of canonical paths. In the case of time-optimal 
control, we might consider paths corresponding to saturated inputs as 
canonical paths, but since it is not clear how to combine these paths to 
get a specific motion, we distinguish these lower level paths from canonical 
paths. Canonical paths have been used by Li and Canny to study the 
motion of a spherical fingertip on an object [60]. This method is discussed 
in Section 4. 


Outline of the chapter 

In remainder of this chapter, we will give a bit more detail about some of 
the various methods of nonholonomic motion planning being pursued in 
the literature. The sections are not organized according to the different 
approaches, but from a pedagogical point of view. In Section 2, we study 
the steering of “model” versions of our nonholonomic system, 


q = g 1 (q)ui-\ - g m (q)um- (8.1) 

By model systems we mean those which are in some sense canonical. We 
use some results from optimal control to explicitly generate optimal in¬ 
puts for a class of systems. The class of systems which we consider, called 
first-order control systems, have sinusoids as optimal steering inputs. Mo¬ 
tivated by this observation, we explore the use of sinusoidal input signals 
for steering second- and higher-order model control systems. This study 
takes us forward to a very important model class which we refer to as 
chained form systems. 

In Section 3, we begin by applying the use of sinusoidal inputs to 
steering systems which are not in a model form. We do so by using 
some elementary Fourier analysis in some cases of systems that resemble 
chained form systems. The question of when a given system can be 
converted into a chained form system is touched upon as well. Then, we 
move on to the use of approximations of the optimal input class by the 
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Ritz approximation technique. Finally, we discuss the use of piecewise 
constant inputs to solve the general motion planning problem. 

In Section 4, we apply the theory developed so far along with some ge¬ 
ometric reasoning techniques involving “canonical trajectories” to study 
repositioning fingers on the surface of a grasped object. 


2 Steering Model Control Systems Using Si¬ 
nusoids 

In this section, we study techniques for steering certain “model” control 
systems: that is, systems which are, in a certain sense, canonical. These 
systems appear to be superficially unrelated to the examples that we 
discussed in Chapter 7, but they are in fact quite relevant, as we shall see 
shortly. The use of sinusoids at integrally related frequencies is motivated 
by the results of Brockett in the context of optimally steering a class of 
systems. This section begins with a review of his results for a class 
of systems whose degree of nonholonomy is two and growth vector is 
(m, m(m + 1)/2). The technique is then extended to certain other classes 
of model control systems with specific attention being paid to a class of 
systems referred to as the so-called “chained form” systems. Techniques 
for steering using methods other than sinusoids and systems other than 
the model systems considered in this section are deferred to Section 3. 

2.1 First-order controllable systems: Brockett’s sys¬ 
tem 

By a first-order controllable system, we mean a control system of the form 
9 = g\{q)ui H-b gm(q)u m , 

where the vector fields gi(q),i = 1 ,,m and their first-order Lie brackets 
[g-j. gk] ■ j < k,k = 1 ,,m are linearly independent and furthermore, we 
have that 

TqW 1 = span{&, \gj,gk] ■ = 

In particular, this implies that n = m + m(m — 1)/2 = m(m + l)/2. A 
very important class of model control systems which satisfy this condition 
was proposed by Brockett [11]. We begin with a discussion of this class 
for the case that m = 2 and n = m(m + l)/2 = 3, 


91 = ui 

92 = «2 (8.2) 
93 = 91^2 - 92^1- 
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For this system, 



1 


'o' 


'o' 

9i = 

0 

92 = 

1 

[91,92] = 

0 


-92 


9i 


2 


Thus the system is maximally nonholonomic with degree of nonholonomy 
2 and growth vector (2,3). We will consider the problem of steering 
system (8.2) from go G R 3 at t = 0 to g/ £ R 3 at t = 1. In fact, we will 
do so as to minimize the least squares control cost given by 


dt. 


Using the fact that g^ = m for i = 1,2, an equivalent description of 
the last equation in (8.2) is as a constraint of the form 

93 = 9i92 - 9291- 

Similarly, the Lagrangian to be minimized can be written as gi+g 2 . Using 
a Lagrange multiplier A (t) for the constraint, we augment the Lagrangian 
to be minimized as follows: 

L(q, q ) = (9i + il) + A(g 3 - gig 2 + g 2 9i)- 

The method for minimizing this constrained Lagrangian is to use the 
classical calculus of variations for the Lagrangian L(g, g) above, with the 
control system written as a constraint with a Lagrange multiplier (the 
reader wishing to learn more about optimal control may consult one of 
several nice books on the subject, such as [123]). There it is shown that 
stationary solutions satisfy the Euler-Lagrange equations. The Lagrange 
multiplier A (t) is determined using the form of the constraint. The fact 
that the equations are precisely the Euler-Lagrange equations of dynamics 
should come as no surprise when one considers that the dynamical equa¬ 
tions may be derived from a least-action principle. The Euler-Lagrange 
equations for minimizing the Lagrangian of our optimal control problem 
are 

d_ / dL(q,q) \ _ dL(q,q) = 
dt V dqi ) dqi 

or equivalently, 

91 + Ag 2 = 0 

92 - Agi = 0 (8.3) 

A = 0. 

Equation (8.3) establishes that A (t) is constant and, in fact, that the 
optimal inputs satisfy the equations: 


ui 


'0 

—A 


Ml 

:= A 

Ml 

u 2 


A 

0 


, U 2. 

. _ 
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Note that the matrix A is skew-symmetric with A constant, so that the 
optimal inputs are sinusoids at frequency A; thus, 


m{t) 


cos A t 

— sin At 

ui( 0 ) 

U 2 (t) 


sin At 

cos At 

_u 2 ( 0 )_ 


Having established this functional form of the optimal controls, given 
values for qo and qf one can solve for the it(0) and A required to steer the 
system optimally. However, from the form of the control system (8.2), 
it is clear that the states qi and q 2 may be steered directly. Thus, it is 
of greatest interest to steer from < 7 ( 0 ) = (0,0,0) to g(l) = (0,0, a). By 
directly integrating for q± and q 2 we have that 


Qi(t) 
.92 (t). 


(e At - J)A-y 0 ). 


Since gi(l) = 92 (f) = 0, it follows that e A = I so that A = 2mr, n = 
0, ±1, ±2,.... Integrating q 3 yields 

f 1 1 

<? 3 (1) = J {qiu 2 -q 2 U 1 )dt = --(ul(0)+ul(0)) =a. 


Further, the total cost is 


! dt = || 22 ,( 0 ) || 2 = —A a. 


Since A = 2nn, it follows that the minimum cost is achieved for n = 
— 1 and that || 22 ,( 0 ) || 2 = 27ra. However, apart from its magnitude, the 
direction of u(0) £ M 2 is arbitrary. Thus, the optimal input steering 
the system between the points ( 0 , 0 , 0 ) and ( 0 , 0 , a) is a sum of sines and 
cosines at a frequency 27 t (more generally if the time period of the 
steering is T). 

The generalization of the system (8.2) to an ?n-input system is the 
system 


<ji =Ui i = 

qij = qiUj - qjUi i < j = 1,..., m. 


(8.4) 


A slightly more pleasing form of this equation is obtained by forming a 
skew-symmetric matrix Y £ so(to) with the —qij as the bottom lower 
half (below the diagonal) to give a control system in R m x so(m): 


q = u 

Y = qu T — uq T . 


(8.5) 


The Euler-Lagrange equations for this system are an extension of those 
for the two input case, namely: 

q - Aq = 0 

A = 0, 
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where A £ so(to) is the skew-symmetric matrix of Lagrange multipliers 
associated with Y. As before, A is constant and the optimal input satisfies 

u = A it, 


so that 

u(t) = e At u(0). 

It follows that e At £ SO(m). It is of special interest to determine the 
nature of the input when q( 0) = q( 1 ) = 0, Y(0) = 0, and U(l) is a given 
matrix in so(m). In this context, an amazing fact that has been shown 
by Brockett [11] is that when m is even and Y is non-singular, the input 
has to /2 sinusoids at frequencies 

27t, 2 • 2-7T,..., to/2 • 2-7T. 

If to is odd, then Y is of necessity singular, but if it is of rank to — 1, 
then the input has (to — l)/2 sinusoids at frequencies 

27T, 2 • 2-7T,..., (to — l)/2 • 2-7T. 

While the proof of this fact is somewhat involved and would take us 
afield from what we would like to highlight in this section, we may use 
the fact to propose the following algorithm for steering systems of the 
form of (8.4): 

Algorithm 1. Steering first-order canonical systems 

1. Steer the q, to their desired values using any input and ignoring the 
evolution of the q^. 

2. Using sinusoids at integrally related frequencies, find uq such that 
the input steers the qij to their desired values. By the choice of 
input, the qi are unchanged. 

The algorithm involves steering the states step by step. The states 
that are directly controlled (zeroth order) are steered first and then the 
first Lie bracket directions are steered. 

2.2 Second-order controllable systems 

Consider systems in which the first level of Lie bracketing is not enough 
to span T g R n . We begin by trying to extend the previous canonical form 
to the next higher level of bracketing: 

qi = Ui i = 1,..., to 

qij = qiUj 1 <i < j <rn 

iijk = qijUk 1 < i,j,k < m (mod Jacobi identity). 


361 


( 8 . 6 ) 


Because the Jacobi identity imposes relationships between Lie brackets 
of the form 


[ 9i, [9j,9k}\ + [9k, [ 9 i, 93 \] + [ 9 3 , [9k, gi}} = 0 

for all i,j, k, it follows that not all state variables of the form of g^/. are 
controllable. For this reason, we refer to the last of the preceding equa¬ 
tions as “mod Jacobi identity”. Indeed, a straightforward but somewhat 
laborious computation shows that 

9231 — 9132 = 91923 — 92913- 

From Exercise 17 of Chapter 7, it may be verified that the maximum 
number of controllable is 

(m + 1 )ro(m — 1 ) 

3 ' 

Constructing the Lagrangian with the same integral cost criterion as be¬ 
fore and deriving the Euler-Lagrange equations does not, in general, result 
in a constant set of Lagrange multipliers. For the case of to = 2, Brock- 
ett and Dai [14] have shown that the optimal inputs are elliptic functions 
(see also the next section). However, we can extend Algorithm 1 to this 
case as follows: 

Algorithm 2. Steering second-order model systems 

1. Steer the qi to their desired values. This causes drift in all the other 
states. 

2. Steer the q-;j to their desired values using integrally related sinu¬ 
soidal inputs. If the ith input has frequency u>i, then will have 
frequency components at u>i ± uij. By choosing inputs such that we 
get frequency components at zero, we can generate net motion in 
the desired states. 

3. Use sinusoidal inputs a second time to move all the previously 
steered states in a closed loop and generate net motion only in 
the qijk direction. This requires careful selection of the input fre¬ 
quencies such that LOi ± uij ^ 0 but u>i + uij + u>u has zero frequency 
components. 

The required calculations for Step 2 above are identical to those in 
Algorithm 1. A general calculation of the motion in Step 3 is quite 
cumbersome, although for specific systems of practical interest the calcu¬ 
lations are quite straightforward. For example, if to = 2 equation ( 8 . 6 ) 
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becomes 


91 = mi 

92 = u 2 
912 = 9iM 2 

9121 = 9l2«l 

9122 = 912M2- 

To steer qi, q 2 , and q\ 2 to their desired locations, we apply Algorithm 1. 
To steer q X2 \ independently of the other states, choose u\ = asin27rf, 
u 2 = b cos Ant to obtain 


9i2i(l) = 9i2i(0) 


a 2 b 

167T 2 


Similarly, choosing u\ = bcosAnt,u 2 = asin27r£ gives 
9122(1) = 9122(0) + 

and all the other states return to their original values. 

Both the algorithms presented above require separate steps to steer 
in each of the qtjk directions. It is also possible to generate net motion 
in multiple coordinates simultaneously by using a linear combination of 
sinusoids and by solving a polynomial equation for the necessary coeffi¬ 
cients (see Exercise 4). 


2.3 Higher-order systems: chained form systems 

We now study more general examples of nonholonomic systems and inves¬ 
tigate the use of sinusoids for steering such systems. As in the previous 
section, we may try to generate canonical classes of higher order systems, 
i.e., systems where more than one level of Lie brackets is needed to span 
the tangent space to the configuration space. Such a development is given 
by Grayson and Grossmann [37], and in [78] we showed that, in full gen¬ 
erality, it is difficult to use sinusoids to steer such systems. This leads 
us to specialize to a smaller class of higher order systems, which we refer 
to as chained systems, that can be steered using sinusoids at integrally 
related frequencies. These systems are interesting in their own right as 
well, since they are duals of a classical construction in the literature on 
differential forms referred to as the Goursat normal form. Further, we 
can convert many other nonlinear systems into chained form systems as 
we discuss in the next section. 
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Consider a two-input system of the following form: 


91 = Ui 

92 = U 2 

93 = 92 M 1 

94 = 93^1 


(8.7) 


Qn — Qn—1^1 • 


In vector field form, equation (8.7) becomes 

9 = giui + g 2 u 2 


with 


1 


'o' 

0 


1 

92 


0 

93 

92 = 

0 

1 - 

3 

1 ’ ‘ 

1 _ 


_0_ 


( 8 . 8 ) 


We define the system (8.7) as a one-chain system. The first item is to 
check the controllability of these systems. To this end, denote iterated 
Lie products as ad k gi gi, defined by: 

ad Sl £ 2 = [ 91 , 92 ], ad^92 = [ 91 , adjj"^] = \gi, [ 91 ,..., [ 91 , 92 ] •]] 


Lemma 8.1. Lie bracket calculations 

For the vector fields in equation (8.8), with k > 1 



0 


(Here the only non-zero entry is in the (k+2)th entry.) 

Proof. By induction. Since the first level of brackets is irregular, we begin 
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by expanding [ 31 , 32 ] and [ 31 , [ 51 , 32 ]] to get 


[91,92} = 


' 0 ' 


' O ' 

0 


0 

-1 


0 

0 

0 

\9i, [91,92]] = 

1 

0 

_ 0 _ 


0 


Now assume that the formula is true for k. Then 

0 


ad£ +1 32 = [ 3 !,ad£ 32] = 


0 

(_l)fe+ i 

0 


□ 


Proposition 8.2. Controllability of the one-chain system 

The one-chain system (8.7) is completely nonholonomic (controllable). 

Proof. There are n coordinates in (8.7) and the n Lie products 
{3i,32,adg 1 3 2 } 1 < i < n — 2 

are independent using Lemma 8.1. Q 

To steer this system, we use sinusoids at integrally related frequencies. 
Roughly speaking, if we use u\ = sin 27rf and u 2 = cos 2irkt then 33 will 
have components at frequency 2n(k — 1 ), 34 at frequency 2ir(k — 2 ), etc. 
qk +2 will have a component at frequency zero and when integrated gives 
motion in qk +2 while all previous variables return to their starting values. 

Algorithm 3. Steering chained form systems 

1. Steer 31 and q 2 to their desired values. 

2. For each qk+ 2 , k > 1, steer qk to its final value using iti = asin27rf, 
u 2 = 6cos27rfcf, where a and b satisfy 

%+2(l) - ft+ 2 (0) = ( —) 
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Proposition 8.3. Chained form algorithm 

Algorithm 3 can steer (8.7) to an arbitrary configuration. 


Proof. The proof is constructive. We first show that using u\ = asin27r7, 
U 2 = bcos2nkt produces motion only in qu +2 and not in qj, j < k+ 2 after 
one period, by direct integration. If q^-i has terms at frequency 2irni, 
then q *. has corresponding terms at 27r(rij ± 1 ) (by expanding products of 
sinusoids as sums of sinusoids). Since the only way to have g*( 1) ^ <?,;(0) 
is to have qt have a component at frequency zero, it suffices to keep 
track only of the lowest frequency component in each variable; higher 
components will integrate to zero. Direct computation starting from the 
origin yields 


<7i 

<72 

<73 


<74 


— (1 — cos27rf), 
2tt 


2nk 


sin 2irkt 


f ab 

J 2irk 

1 ab 

2 2ixk 

1 


sin 2irkt sin 27 t t dt 

sin27r(fc — l)t sin 27r(fc + 1)7 
27r(fc — 1) 27 t(/c + 1) 

o?b f 

/ sin 2n{k — 1)7 • sin 2irt dt + • 


2 2nk ■ 2n r(fc — 1) 
1 a 2 i 


2 2 2-nk ■ 2ix(k — 1) • 27r(fc — 2) 


sin 27r(fc — 2)7 + ■ 


«« = / Mdt + ' ’ ’ 

1 a k b t | 

“ 2 fe - x (27r) fe fc!2 + "’ 

It follows that <? fc _|_ 2 (1) = < 7 fc+ 2 ( 0 ) + (^) fc |r and all earlier qf s are periodic 
and hence <?i(l) = 9i(0), i < k. If the system does not start at the origin, 
the initial conditions generate extra terms of the form gi_i( 0 )iti in the 
ith derivative and this integrates to zero, giving no net contribution. □ 

For the case of systems with more than two inputs, and to the so-called 
multi-chained form systems, we refer the reader to [78]. 


3 General Methods for Steering 

Model control systems of the kind that we discussed in the previous sec¬ 
tion will very seldom show up verbatim in applications. In this section, 
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we consider some techniques in motion planning for more general non- 
holonomic systems. 


3.1 Fourier techniques 

The methods involving sinusoids at integrally related frequencies can be 
modified using some elementary Fourier analysis to steer systems which 
are not in any of the model classes that we have discussed. We will 
illustrate these notions on two examples that we studied in Chapter 7. 

Example 8.1. Steering the hopping robot in flight 

We saw in Chapter 7 that tp was the angle of the hip of the hopping robot 
in the flight phase, l the length of the leg extension, and 8 the angle of 
the body of the robot. The control equations are given by 


Ip = U\ 

l = u 2 


m(l + d) 2 
/ + m(l + d) 2 


Ml. 


(8.9) 


Expanding the right-hand side of the last equation of (8.9) in a Taylor 
series about l = 0, we get 


8 = - 


md 2 

md 2 +I^ 


2 mdl 
( md 2 + I) 2 


lui + 0(^ 2 )ui 


where 0(l 2 ) stands for quadratic and higher-order terms in l. This sug- 

,2 

gests a change of coordinates of the form a = 8 + ^ 2 +j ip to put the 
equations in the form 


= Mi 

l = m 2 

If one neglects the higher order terms in the last equation, this equation 
has the form of a chained form system with 3 states. Using this as 
justification, we attempt to use the algorithm for steering chained form 
systems to steer the precise form of this system. We first steer the %p and 
l variables to their desired values. Then, we use sinusoids 


Mi = a\ sin 2irt 
m 2 = a 2 cos 2nt 


to steer a. By choice, after one period (1 second), the last motion does 
not affect the final values of V’ and l. Since l = " 2 t_ sin27rt over this piece 
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of the motion, we can expand f{l) by its Fourier series as 
/(— sin 2tt t) = (3\ sin 2nt + @2 sin int + • • • 


Integrating a over one period and noting that only the first term con¬ 
tributes to the net motion, yields 



= 


Since (3\ is a function of 02 , one can solve for a\, a 2 numerically to achieve 
a net change in a. Cartwheeling in mid-air consists of a net change in 
phase of 2n radians. 

Example 8.2. Steering the kinematic car 

The equations for the kinematic model of a front wheel drive car are given 
by (7.14), namely 


x = cos 9 u\ 
y = sin 0 U\ 

9 = — tan cf> u\ 


= u 2 - 


In this form, u\ does not control any state directly. We use a change of 
coordinates z\ = x, Z2 = 4>, £3 = sin 0 , Z4 = y, and a change of inputs 
v\ = cos 9 u\ , V2 = U2 to put the equations in the form 


Zi = Vi 

ii = V2 



As in the previous example, the linear terms in the Taylor series expan¬ 
sions of the nonlinearities in the last two equations match the terms of 
the one-chain system, and we can include the effect of the nonlinear terms 
using Fourier analysis. 

An example of the application of Algorithm 2 applied to the car is 
given in Figure 8.1. The first part of the path, labeled A, drives x and 
to their desired values using a constant input. The second portion labeled 
B, uses a sine and cosine to drive 9 while bringing the other two states 
back to their desired values. The last step labeled C, involving the inputs 


ui = ai sin 4 - 7 rf U 2 = CL 2 sin 2iit, 
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Figure 8.1: Sample trajectories for the motion of a car. The trajec¬ 
tory shown is a sample path which moves the car from (a :,y,0,<f>) = 
(—5,1,0.05,1) to (0, 0.5,0,0). The first three figures show the states ver¬ 
sus x , the bottom right graphs show the inputs as functions of time. 


brings y to the desired value and returns the other states back to their 
correct values. The Lissajous figures obtained from the phase portraits of 
the different variables are quite instructive. Consider the part of the curve 
labeled C. The upper left plot contains the Lissajous figure for x, <f> (two 
loops); the lower left plot is the corresponding figure for x,9 (one loop); 
and the open curve in x, y shows the increment in the y variable. The 
interesting implication here is that the Lie bracket motions correspond 
to rectification of harmonic periodic motions of the driving vector fields, 
and the harmonic relations are determined by the order of the Lie bracket 
corresponding to the desired direction of motion. 


3.2 Conversion to chained form 

An interesting question to ask is whether it is possible using a change of 
input and nonlinear transformation of the coordinates to convert a given 
nonholonomic control system into one of the model forms discussed in 
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the previous section. More precisely, given the system 
q = g\{q)ui H-b g m (q)um, 


does there exist a matrix /3(q) £ R. mxm anc j a diffeomorphism (f> : K” 
R” such that with 


v = (3(q)u z = (f>(q) 


the system is in chained form in the 2 : coordinates with inputs v ? One 
can give necessary and sufficient conditions to solve this problem (see 
[74]), but the discussion of these conditions would take us into far too 
much detail for this section. In [78], we gave sufficient conditions for the 
two-input case, in which case the system was to be converted into the one- 
chain form. The conditions assume that gi,g 2 are linearly independent. 
Now, the system can be converted into one-chain form if the following 
distributions are all of constant rank and involutive: 


A 0 = span{gi, g 2l ad Sl g 2 , ■.., ad ” -2 31 } 
Ai = span{g 2 , ad Sl g 2 ,..., ad " -2 g 2 } 

A 2 = span{ 3 2 , ad Sl g 2 , ..., ad "~ 3 g 2 } 


and there exists a function hi(q) such that 


dhi ■ Ai = 0 and dh\ ■ g\ = 1. 


If these conditions are met, then a function h 2 independent of hi may be 
chosen so that 


dh 2 ■ A 2 = 0. 


The existence of independent hi and h 2 so that dhi ■ Ai = 0, dh 2 • A 2 = 0 
is guaranteed by Frobenius’ theorem, since A 2 C Ai are both involutive 
distributions. There is however an added condition on hi, namely that 
dhi ■ gi = 1. If we can find these functions hi,h 2 , then the map (f> : q 1 —> z 
and input transformation given by 


zi = hi 


Vi := Ui 



v 2 (Lg ± 1 h 2 )u 1 + (L g2 L^ ji 2 h 2 )u 2 


Zn—l — -^91 ^2 
Zn = h 2 


yields 


Zi = Vi 


z-l = v 2 


Z3 = Z 2 Vi 


Z n — Zn— 1^1 • 
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This procedure can be illustrated on the kinematic model of a car. 


Example 8.3. Conversion of the kinematic car into chained form 

First, we rewrite the kinematic equations as 


x = u i 
y = tan 0 u\ 

0 = - tan (f> u\ 


<t> = u 2 - 


Then, with h\ = x and h 2 = y. it is easy to verify that this system 
satisfies the conditions given above and the change of variables and input 
are given by 



Z 3 = tan 9 
Z4=y 

to give a one-chain system. 

3.3 Optimal steering of nonholonomic systems 

In this section, we discuss the least squares optimal control problem for 
steering a control system of the form 


q = g\{q)ui H- V g m {q)um 


from qo to qf in 1 second. Thus, we minimize the cost function 



Our treatment here is, of necessity, somewhat informal; to get all the 
smoothness hypotheses worked out would be far too large an excursion 
to make here. Wc will assume that the steering problem has a solution 
(by Chow’s theorem, this is guaranteed when the controllability Lie al¬ 
gebra generated by g\,.... g rn is of rank n for all q). We give a heuristic 
derivation from the calculus of variations of the necessary conditions for 
optimality. To do so, we incorporate the constraints into the cost function 
using a Lagrange multiplier function p(t) G R” to get 
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Introduce the Hamiltonian function: 


H(q,p,u) = -u T u+p T '^2g i (q)u i . 


( 8 . 11 ) 


Using this definition and integrating the second term of (8.10) by parts 
yields 

1 f 1 

J{q,P,u) = -p T (t)q{t) + (H(q,p,u) + p T q)dt. 

o Jo 

Consider the variation in J caused by variations in the control input u, 1 

SJ = -p T (t)Sq(t) + f ( ®^5q + + p T 5q\ dt. 

o Jo \oq au J 

If the optimal input has been found, a necessary condition for stationarity 
is that the first variation above be zero for all variations Su and 5q: 


dH dH n 

P ~~~d^ ~dJI ~ ‘ 


( 8 . 12 ) 


From the second of these equations, it follows that the optimal inputs are 
given by 

m = -p T gi{q ), i = 1,... ,rn. (8.13) 

Using (8.13) in (8.11) yields the optimal Hamiltonian 


H*(q,p) = {p T 9i(q)Y ■ 


(8.14) 


i=l 


Thus, the optimal control system satisfies Hamilton’s equations: 

dH* 


q = 


-{q^p) 


dp 
dH* 

P = -^ q ’ p) 


(8.15) 


with boundary conditions g(0) = qo and q( 1) = g/. Using this result, we 
may derive the following proposition about the structure of the optimal 
controls: 


Proposition 8.4. Constant norm of optimal controls 

For the least squares optimal control problem for the control system 

m 

q = ^2gi{q)ui 

i=l 

the calculus of variations, one makes a variation in it, namely Su, and calculates 
the changes in the quantities p and H as Sp and SH. See for example [123]. 
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which satisfies the controllability rank condition, the norm of the optimal 
input is constant, that is, 


Kt )|| 2 = |KO )|| 2 vte[o,i]. 


Proof. The formula for the optimal input is given in (8.13). Differentiat¬ 
ing it yields 



Further, using the Hamiltonian equation for fik given by 



it may be verified that the formula for tq is given by 


m 



Collecting these in a matrix gives 


u = f l(q,p)u 


(8.16) 


where Q(q,p) is a skew-symmetric matrix (i.e., it is in so{m)) given by 


P T [9i,92] 

0 


P r [9i,9m ] 
P [92,9 m] 


0 

P T [91,92] 


M(q,p) 


-p T [9l,9m] -P T [92,9m] ■■■ 0 


The solution of the linear time-varying equation (8.16) is of the form 


u(t) = U (t)u( 0) 


(8.17) 


for some U(t) £ SO(m) (see Exercise 6). From this fact the statement of 


the proposition follows. 


□ 


This proposition provides an interesting formula (8.16) for the deriva¬ 
tives of the optimal input and establishes that the norm of the optimal 
input is constant. This fact can be used to establish that the same optimal 
input also solves other optimization problems which involve a monotone 
transformation of the integrand, such as 



as well as some minimum-time problems (see Exercise 8). This proposi¬ 
tion can be used to solve certain optimization problems, such as that for 
the so-called Engel’s system: 
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Example 8.4. Optimal inputs for Engel’s system 

This system is of the form 


91 = Ui 

92 = U 2 

93 = 9lW2 - 92«1 

94 = 9]W 


This system has growth vector (2,3,4). It may be verified that 


[ 91 , 92 ] 


0 

0 

2 


_29i_ 

so that the optimal inputs satisfy the differential equation obtained by 
specializing (8.16), namely 


Ul = 2(p 3 + qiP4)u 2 

U2 = -2(p 3 + q\Pi)u\. 


(8.18) 


The solution of this equation is of the form 

U\{t) = r sina(f) 

U 2 (t) = r cosa(f), 

where r 2 = u\ (0) + u|(0). Further, since the optimal Hamiltonian given 

by 1 1 

H*{q,p) = — — (pi — 92P3) 2 - 2^2 + 9 iP 3 + qlPif 

is independent of 93 and 94 , it follows that P 3 = P 4 = 0 so that p^ and P 4 
are constant. Using the functional form of u\ and U 2 in (8.18) we get 


a = 2 p^r sin a. 


To integrate this equation, multiply both sides by 2d, integrate and define 
5 = 2 p 4 f to get 

(d) 2 = b — 28 cos a, 

where b is a constant of the integration. If b — 2\8\ > 0, this equation can 
be written as 


d = ±\/b — 28 cos a = c 



for some constants c, k. This last equation may be integrated using ellip¬ 
tical integrals (see, for example, Lawden [56]) for a using 



da 

\/l — k 2 sin 2 


a 



(8.19) 
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The left hand side of this equation is an elliptic integral. Hence the 
optimal inputs for the Engel’s system come from elliptic functions. 

In general, it is difficult to find the solution to the least squares op¬ 
timal control problem for steering the system from an initial to a final 
point. However, it may be possible to find an approximate solution to 
the problem by using the so-called Ritz approximation method. To ex¬ 
plain what this means, we begin by choosing an orthonormal basis for 
L 2 [0,1], the set of square integrable functions on [0,1]. One basis is a set 
of trigonometric functions {ip 0 (t),ipi{t ),...} given by ^o(^) = 1 and for 
k> 1, 


t/> 2 fc-i(t) = V2cos2knt, ip 2 k(t ) = v / 2sin2/c7rf t £ [0,1]. 

For a choice of integer N, a Ritz approximation to the optimal ith input 
is assumed to be of the form 


N 

u i(t) = y v ,aifcy>fc(t)- 

k—0 

The plan now is to apply this input to steer the system 

q = 9 i(q)ui-\ - \-gm{q)u m (8.20) 

from q(0) = qo to g(l) = qf and to determine the coefficient vectors, 
aj = (atio, an, .. ., an v) £ K Ar+1 for * = 1,..., to, so as to minimize the 
cost function 

.. m 

J= £(£ INI 2 +7ll«( 1 )-s/ll 2 )- ( 8 - 21 ) 

i=1 

In equation (8.21) the coefficient 7 > 0 is a penalty term corresponding to 
reaching the final state. For large 7 the cost function weights the reaching 
of the goal heavily. The heart of the Ritz approximation procedure is the 
hope that for large N and large 7 , we will find a u that steers to the final 
point qf with low cost. 

At this point, we have a finite-dimensional optimization problem, 
which may be solved by a variety of methods. One method involving 
a modification of the Newton iteration for minimizing J has been ex¬ 
plored in [32] and [33], where a software package called NMPack for this 
purpose is described. We refer the reader to these papers for details of 
its application to various examples. 

3.4 Steering with piecewise constant inputs 

In this section, we describe the rudiments of a method for motion planning 
for general nonholonomic systems due to Lafferriere and Sussmann [54]. 
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The algorithm works for systems whose controllability Lie algebra is nilpo- 
tent of order k. By way of review, this means that for systems of the form 

X — QlUi T * * * T 0m^m 

the Lie products between control vector fields of order greater than k are 
0; i.e., [gt 1 , ..., [(Ji p _ 1 , 0 ,;J...] is zero when p > k. The method proposed 
in [54] is conceptually straightforward but the details of their method are 
somewhat involved. The first step is to derive a “canonical system equa¬ 
tion” associated with the given control system. The chief new concept 
involved is that of formal power series in vector fields. 

Recall from Section 2 of Chapter 7 that the flow associated with a 
vector field gi was denoted referring to the solution of the differ¬ 

ential equation q = gi(q) at time t starting from q at time 0. This flow is 
referred to as the formal exponential of gi and is denoted 

e t 9 i {q) := (f)f (q). 

The usage of the formal exponential is in that we will actually use iden¬ 
tities of the form 

1 2 

e tg, = (/ + tgi + —g i -|-), 

where polynomials like gf and gf need to be carefully justified. We will 
defer this question for the moment and think formally of e t9i ( q ) as a 
diffeomorphism from R” to R”. Now, consider a nilpotent Lie algebra 
of order k generated by the vector fields gi ,..., g m . Recall from Section 
4 of Chapter 7 that a Philip Hall basis is a basis for the controllability 
Lie algebra which has been constructed in such a way as to keep track of 
skew-symmetry and the Jacobi identity associated with the Lie bracket. 
We define the Philip Hall basis of the controllability Lie algebra generated 
by fll,.. .,Pm to be 

B \, B 2 ,..., B s . 

Thus, basis elements are Lie products of order less than or equal to k. In 
our language of formal power series, we will refer, for instance, to 


:= 0102 — 0201 

[01, [02, 03]] := 010203 — 010302 — 020301 + 030201- 

It is a basic result of nonlinear control, called the Chen-Fliess series 
formula, that all flows of the nonlinear control system ( 8 . 1 ), namely, 

m 

d = ^2ai{q)uz 0 ( 0 ) = q 

i=1 


are of the form 


S t (q) = efc.WB.efc.-itoB .- 1 ... 


( 8 . 22 ) 
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for some suitably chosen functions hi, / 12 , ■ ■■, h s , known as the Philip 
Hall coordinates. The meaning of equation (8.22) is as follows: all flows 
that could possibly be generated by the control system of (8.1) may 
be obtained by composing flows along the Philip Hall basis elements 
B \,..., B s . This result bears more than a passing resemblance to the 
product of exponentials formula for manipulator kinematics, but we will 
not digress to make this connection more explicit here. Furthermore, 
St(q) satisfies a differential equation involving the basis elements, namely, 

S(t) = S(t)(B 1 vi + --- + B s v a ) 5(0) = 1, (8.23) 

where St (q) has been replaced by 5 (t) and the inputs v \,..., v s are the 
“fictitious inputs” corresponding to the directions of the Philip Hall ba¬ 
sis elements B \,..., B s . We say fictitious since only the first m of the 
Philip Hall basis elements correspond to gi,... ,g m - The other inputs 
correspond to Lie bracket elements and will eventually be dropped. Dif¬ 
ferentiating equation (8.22) yields 


s(t) = j2^ Bs 

3 =1 


e hjBj hjBj e hj - lBj ~ 1 ■ ■ ■ e hlBl 


S 

Y S(t)e~ hlBl ■ ■ • hjBj 

3 = 1 


■— 5(t) hjBj. 

f=i 


e hlBl 


(8.24) 


Here, in analogy with the formulas of Chapter 2, we have introduced the 
notation 

Ad e -h iBi Bj = e~ hiBi Bje hiBi . 

Since the controllability Lie algebra is nilpotent of degree k, we can ex¬ 
press each one of the elements on the right hand side in terms of the basis 
elements B \,..., B s . More specifically, it may be verified that 


Ad e -h 1 B 1 ... e -hj_ 1 B j _ 1 = Ad e -ii!B j •••Ad e -hj_ 1 Bj_ 1 . (8.25) 

Thus each element on the right hand side of (8.24) is a linear combination 
of Bi,... ,B S and we may express 

A.d e -h 1 B 1 ... e - h j-iB j _i hjBj = ^Y,Pj,k(h)Bk^ hj 

for some polynomials pj } k{h). Using this in the equation (8.23) and equat¬ 
ing coefficients of the basis elements Bi yields 

S 

Y.Pi.mhj = v k k = i,...,s. 
j=i 
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These equations are then solved to give the differential equation 

h = Q{h)v h( 0) = 0 (8.26) 


which is a control system in R s , called the Chen-Fliess-Sussmann equa¬ 
tion, specifying the evolution of the Philip Hall coordinates in response 
to the “fictitious inputs” vi,...,v s . It is important to realize that, in 
general, the dimension s of the Philip Hall coordinates is greater than 
n, the dimension of the state space of the control system. The initial 
conditions are hi(0 ) = 0 corresponding to the identity diffeomorphism at 
t = 0. This equation is the canonical form associated with the nilpotent 
controllability Lie algebra associated with the given problem. 

Example 8.5. Nilpotent system of degree three with two inputs 

Consider a two-input system which is nilpotent of degree three on R 4 . 
We will assume that g i, <72 , [51,52]) [51, [51,52]] are linearly independent. 
As the Philip Hall basis, we have 

Bi =51 B 2 = 52 B 3 = [51,52] 

Ba = [5i, [51,52]] B 5 = \ g 2 , [51,52]]. 


Since [ 52 , [ 51 , 52 ]] = B$ is dependent on B\, B 2l B 3 , B4 by hypothesis, we 
have in (8.23) that v 3 = 0. An easy calculation shows that the coefficients 
of the hj on the right hand side of (8.24) are given by 


hi: 

B 1 



h 2 : 

b 2 

— h 3 B 3 

+ \h\B. 

h 3 : 

b 3 

— h 2 B 3 

— h 3 B4 

/14: 

B4 



h 5: 

B 5 . 




For instance, the coefficient of h 2 is calculated as 

Adg-^iB! B 2 = B 2 — hi[B\,B 2 ] + -h\[B\, [Bi,B 2 ]\ 

= B 2 — h\B 3 + -h\Bi. 

Equating the coefficients of the Bi to Vi with v$ = 0, we get the Chen- 
Fliess-Sussmann equation 


hi = vi 
h 2 = v 2 

h 3 = hiv 2 + v 3 

1 2 

hi = 7^h 1 v 2 + hiv 3 + V4 
h§ = h 2 v 3 + hih 2 v 2 . 


(8.27) 
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Note that this system is in R 5 , though the state space equations evolve 
on R 4 . 

Example 8.6. Two-input, five-state chained system 

Consider a chained system with two inputs and five states, where the 
input vector fields are 


' 1 ' 


'O' 

0 


1 

92 

92 = 

0 

93 


0 

. 94 . 


0 


The system is nilpotent of degree k = 4 , and the Philip Hall basis vectors 
are 


Bi,B 2 : 

9 i 92 

B 3 : 

[91,92] 

f? 4 , B 5 : 

[91, [91,92]] [92, [91,92]] 

f? 6 , Bj : 

[ 9 i, [ 9 i, [91,92]]] [92, [91, [91,92]]] 

Bs : 

[92, [92, [91,92]]] 


The vector fields 91 , g 2 , 33 := ad ffl g 2 , 94 := ad^ g 2 , and g 6 := a.d 3 gi g 2 
span the tangent space of R 5 . 

Thus, for the Chen-Fliess-Sussmann equations, we have v$ = V 7 = 
Vg = 0 , and the coefficient of hj is given by 

Ad e -h 1 B 1 • • • Ad e -hj_ 1 Bj _ 1 Bj , j = 1,... . 8 . 

We carry out the calculation for h 3 in detail: 

Adg-hjBj Adg-fi 2 ®2 B 3 

= Ad e -h 1 s 1 ^B 3 — h 2 B 3 + h^BsJ 
= B 3 — h±Bx — h 2 B§ T -hiBe h\h 2 B r j T —h^Bs- 

The calculations for the remaining terms are carried out in a similar 
fashion and the results are given below (we invite the reader to do the 
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calculation herself): 


hi : B\ 

h-2 '■ B 2 — h 3 B 3 + — -h\B§ 

2 b 

h 3 : B 3 — h 3 B^ — h 2 B 3 — — Ji^Bq — h\h 2 B r j — — 

/i-4 * -S4 — H\B§ — /i2-^7 

h §: -B 5 — /11-B7 — h 2 B s 

h,Q: Bq 

hf: By 

h$: B%. 

Finally, with v 3 = v? = vg = 0 the differential equation for the h £ M 8 is 
found to be 

hi = td 
h 2 = v 2 

h 3 = h 3 v 2 + v 3 

ii4 = \h\ y 2 + hiV 3 + M 4 

h 5 = h\h 2 v 2 + h 2 v 3 

1 o In 

Hq = q^ 1 v 2 + 2^ llV3 + ^ lV 4 + ^6 
h 7 = —h^h 2 v 2 + h 3 h 2 v 3 + h 2 V4 

h 8 = -hih 2 v 2 + -hlv 3 
with initial condition h(0) = 0. 


The task of steering the system from q 3 to qf still remains to be 
done. This is accomplished by choosing any trajectory connecting go to 
qf indexed by time t. This is substituted into (8.23) to get the expression 
for the “fictitious inputs” v\,... ,v n , corresponding to the first n linearly 
independent vector fields in the Philip Hall basis of the control system. 
The inputs Vi are said to be fictitious, since they need to be generated by 
using the “real” inputs Mj, i = 1,..., m. To do so involves a generalization 
of the definition of Lie brackets given in Section 2 of Chapter 7. For 
instance, to try to generate an input v 3 corresponding to [ 51 ,^ 2 ] i n the 
previous example, one follows the definition and uses the concatenation of 
four segments, each for y / e seconds: iq = l,u 2 = 0; u\ = 0 ,M 2 = l;ui = 
— 1, u 2 = 0; Mi = 0, u 2 = —1. The resulting flow is given by 


gV^Sl e V^92 g- VeSl g- V^92 


(8.28) 
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A formula called the Campbell-Baker-Hausdorff formula gives an expres¬ 
sion for this in terms of the Philip Hall basis elements. We will make a 
brief digression to state this formula, since it is important in its own right 
(the proof and a more exhaustive recursive formulation of the coefficients 
is given in [ 115 ]). 

Theorem 8.5. Campbell-Baker-Hausdorff formula 

Given two smooth vector fields g\ , 52 the composition of their exponentials 
is given by 

e 9i e 92 = e 9i+92 + 3 [ 01 ,02] + j3(bi> [91,92]] - [02,[0i,52]])--- ( 8 29 ) 

where the remaining terms may be found by equating terms in the (non- 
commutative) formal power series on the right- and left-hand sides. 

Using the Campbell-Baker-Hausdorff formula for the flow in equa¬ 
tion ( 8 . 28 ) gives the Philip Hall coordinates to be 

h = (0, 0, e, /14(e)), 

where /14(e) is of higher order than one in e. Thus, the strategy of cy¬ 
cling between u\ and U 2 not only produces motion along the direction 
[<71, <72] 1 but also along [g\ , [51,02]]- Thus, both 113, U4 ^ 0 . However both 
the hi and /12 variables are unaffected. To generate motion along the V 4 
direction, one concatenates an eight-segment input consisting of cycling 
between the iti,u 2 for ^[i seconds. Since the controllability Lie algebra 
is nilpotent, this motion does not produce any motion in any of the other 
bracket directions (in the context of the Campbell-Baker-Hausdorff for¬ 
mula above, the series on the right hand side of ( 8 . 29 ) is finite). This 
example can be generalized to specify a constructive procedure for gener¬ 
ating piecewise constant inputs for steering systems which have nilpotent 
controllability Lie algebras. 

When the controllability Lie algebra is not nilpotent, the foregoing 
algorithm needs to be modified to steer the system only approximately. 
The two difficulties in this case are: 

1 . The Philip Hall basis is not finite. 

2 . The Campbell-Baker-Hausdorff formula does not terminate after 
finitely many terms. 

One approach that has been suggested in this case is to try to change 
the input gi using a transformation of the inputs, that is by choosing a 
matrix (3 £ R mxm so as to make the transformed gi , defined by 

[51 92 ■■■ g m \= P [91 92 ■■■ 9m] 

a nilpotent control system. Other approaches involve “nilpotent approx¬ 
imations” to the given control system. For more details on the algorithm 
for steering nonholonomic systems using piecewise constant inputs, see 
the paper of Lafferriere and Sussmann [ 54 ]. 
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Figure 8.2: Three-fingered hand grasping an object. The small circles be¬ 
low each finger indicate the internal forces applied by each finger. (Figure 
courtesy of John Hauser) 

4 Dynamic Finger Repositioning 

We return now to the problem of repositioning the fingers on a hand 
without actually lifting the fingers from the object. As we saw in the 
examples in Section 4 of Chapter 7, if we have spherical fingertips con¬ 
tacting a planar face, it is possible to move the contact point via pure 
rolling. This section works out this problem in detail and gives several 
different solutions for the dynamic finger repositioning problem. 

4.1 Problem description 

Consider the grasping control problem with rolling contacts, such as the 
system shown in Figure 8.2. In Chapter 5, we derived the kinematic 
equations of motion for a single body in contact with a set of fingers. In 
local coordinates, the overall constraints on the system have the form 

Jh(9, x)9 = G t {9 , x)x, (8.30) 

where 9 is the vector of finger joint angles and x specifies the position 
and orientation of the grasped object. 

If the grasp described by the constraints in equation (8.30) is manip- 
ulable, then any object velocity x can be accommodated by some finger 
velocity vector 9. However, the vector 9 may not be unique in the case 
that the null space of Jh is nontrivial. This situation corresponds to the 
existence of internal motions of the fingers that do not affect the motion 
of the object. If we let u\ be an input which controls the velocity of the 
object and let 112 parameterize the internal motions, then equation (8.30) 
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can be written as 


x = u i 

7 + 


— Jfa G U\ + Ku2i 


(8.31) 


where the columns of K span the null space of Jh- 

Equation (8.31) describes the grasp kinematics as a control system. 
The dynamic finger repositioning problem is to steer the system from 
an initial configuration (Oo,Xq) to a desired final configuration ( Of,Xf ). 
The explicit location of the fingertip on the object at the initial and final 
configurations can be found by solving the forward kinematics of the 
system. 

The general case of finding u\ (t) and u 2 ( t ) such that the object and the 
fingers move from an initial to final position (while maintaining contact) 
can be very difficult. We point out two interesting special cases: 

1. If the hand has no redundant degrees of freedom (i.e., K is not 
present) then it might be possible to move to an arbitrary loca¬ 
tion/grasp using only u\. Moving just the contact location requires 
a carefully chosen closed loop path in x. 

2. If we have redundant degrees of freedom, then we can move the 
fingers along the object while keeping the object position fixed (x = 
u\ =0). In this case, we use only the vector fields in K to move 
the fingers. 

In the second case, it is sufficient to study the control of a single finger, 
since the fingers are decoupled if the object is held fixed. We concentrate 
here on the second case, which is considerably simpler. 


4.2 Steering using sinusoids 

Consider the case of a single spherical finger rolling on a plane. The 
kinematics were derived in Chapter 7 and the associated control system 
is repeated here: 


1 = 


qi 


0 


-1 

<72 


sec q\ 


0 

<73 

= 

- sin q 5 

Ml + 

— cos q 3 

<74 


— cos q 5 


sin q 5 

_<k_ 


— tan qi 


0 


u 2 . 


(8.32) 


As in the preceding discussion, we will change variables to new ones and 
keep track of the Taylor series expansions of the nonlinear terms to get 
a two-chained system. More specifically, with change of state 


Zi =qi z 2 = q 2 z 3 = -q 5 z 4 = q 3 - q i z 5 = q 4 + q 2 
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Figure 8.3: Steering applied to a multifingered hand. 


and change of input 


v\ = — u 2 v 2 = sec qiU\ 


we get 

Zi = Vi 

z 2 = v 2 

z 3 = z\v 2 + ipi(zi)v 2 (8.33) 

Zi = Z 3 V 2 + 4> 2 {z 3 )vi + 1p(zi,Z 3 )V2 

Z5 = Z 3 V\ + i>4{z 3 )V i + ^5(Z1,Z 3 )V2, 

where the ipi are quadratic or higher order in their respective arguments. 
If these functions i />* are neglected, then we have a two-chained system 
which can be steered using sinusoids. Unlike the case of the hopping 
robot or car parking, the calculation of the Fourier series coefficients for 
Z 4 and z 5 is not easy. However, a numerical procedure based on using a 
sines and cosines at integrally related frequencies may be used to steer the 
finger on the surface of the object. An example of such a path moving a 
spherical fingertip down the side of a planar object is shown in Figure 8.3. 
In this figure, we consider the motion of a finger with a spherical tip 
on a rectangular object (left). The plots to the right of the figure show 
trajectories which move a finger down the side of the object. The location 
of the contact on the finger is unchanged as shown in the upper graph 
which plots the finger contact configurations ( 91 ,( 72 ) = ( u fi v f), while 
the location of the contact on the face of the object ( 93 , 94 ) = (u 0 ,v 0 ) 
undergoes a displacement in the v 0 direction. 
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4.3 Geometric phase algorithm 

In this subsection, we will describe the use of some techniques from clas¬ 
sical differential geometry which can be brought to bear on the specific 
problem of rolling a spherical finger on a planar surface. 

As in Chapter 5, let Q be the configuration space of contact, Sf and So 
the surface of the fingertip and the object. Then, in local coordinates, Q 
is parameterized by p = (aj,a 0 ,)/j), where ay = ( Uf,Vf),a 0 = (u 0 ,v 0 ) £ 
R 2 are local coordinates on Sf and S a respectively, and ip is the angle of 
contact. The kinematic equations of rolling contact from Chapter 5 are 


Mfaf — RpM 0 a 0 = 0 
TfMfdtf + T 0 M 0 d 0 — ip = 0. 


(8.34) 


In the instance of a spherical finger rolling on a plane, we have that 
M a = I, T a = 0 and that 


T f=[ 0 ~7 tanu /] 


M f = 


0 

p COS U f 


where p is the radius of the finger. Since Mf and RpM a are nonsingular, 
given either af(t) £ R 2 or a 0 {t) £ R 2 there exists a unique path p(t) £ Q 
which satisfies the rolling without slipping constraint. More specifically, 
let a/(f),f £ [0,1], be a path in Sf and denote u = df. Then the path 
rj(t) £ Q is given by integrating the following differential equations 


d f = u 

d 0 = M~ 1 RpMfu (8.35) 

ip = (Tf + T 0 R l p)MfU. 

This is to say that there exists a well defined lifting map p~ l : S / —> Q 
which lifts every path in Sf to a path in Q. The following classical 
theorem describes how a closed path in Sf generates a change in the 
angle of contact ip. Using the formulas for T/,M/ and T a above yields 

ip = —smufU2- (8.36) 

Note that this equation is independent of the radius p of the sphere. Thus, 
the following theorem, though stated for spheres of radius one, actually 
holds for spheres of arbitrary radius. 

Theorem 8.6. Gauss-Bonnet theorem 

Let af : [0,1] —> Sf be a closed path on the sphere of radius one which 

encloses a cap shaped region ft as shown in Figure 8.f. Let Aip be the 

change of the angle of contact as a result of rolling the sphere on the plane 
along the path given by ci/(-)- Then 

Aip = — Area of U, (8.37) 
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Figure 8.4: Geometric phase of a path in the sphere. 


where the area of Q is measured on the (curved) surface of the sphere of 
radius one. 

Proof. From equation (8.36), we have that 

ip = TfMfdf = — smufi>f. 


Integrating ip along the curve «/(•) and applying Green’s theorem to the 
line integral yields 


A ip 



sin Uf dvf 



cos Uf dvf duf 


— Area of 


n, 


since cos Ufdufdvf is the infinitesimal area (area form) on a sphere of 
radius one in local coordinates. Q 

In the preceding theorem, we saw that the net change in the contact 
angle A ip depends on the area enclosed by the curve «/(•) on a sphere of 
radius one regardless of the actual radius of the sphere ! A ip is referred to 
as the geometric phase or nonholonomy of the path ctf(-). It tells us the 
motion (in fact the area to be covered by a closed path traced out by the 
finger) to generate a certain change in the contact angle A ip. 

In the next proposition, we show how to generate motion on the spher¬ 
ical finger by using a closed path a 0 on the planar surface of the object. 
This specifies the motion of the object surface required to reposition the 
finger. In turn, we may use this method to produce closed loops in the 
motion of the finger, which produce changes in phase Aip. The devel¬ 
opment is completely geometric and does not use the rolling equation 
(8.35). 

Proposition 8.7. Sphere rolling on a plane 

Let cl f and af be two points on the sphere which are a distance l apart 
on the sphere. Assume that l < ^. Then, rolling the sphere on the 
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Figure 8.5: Closed path in the plane generates motion on the sphere. 

plane along the closed curve ABCDA shown in Figure 8.5 takes a/ to af 
provided the segments AB, DA are of length x and BC, CD are of length 
^, and x solves the equation 



2 I x — tan 1 


Proof. Connect a/ and <5/ by an arc of a great circle, and denote by A 
the starting point on the plane. Tracing the straight line from A to B 
and then to C on the plane induces a curve in the sphere which starts at 
a/, passes through B' , and then comes to the north pole, C. as shown 
in the Figure 8.5. Since the distance traversed by the fingertip on the 
surface of the object is the same as the distance on the sphere, we have 
that the distance between a/ and B' is x and Z(afB'C') = f. Going 
from C to D at an angle 6 and a distance ^ is equivalent to going down 
from C’ to some point D' on the equator and d(B\ D') = 9. By tracing 
a straight line from D to A on the plane, we follow the equator from D' 
to some point o.f, where d(a.f,D') = x. It is clear from the figure that 



Furthermore, for each l < | the following equation 

f( x ) = l 


has a unique solution x £ [0, |] because /(0) = 0,/(7t/2) = 7r/2 > l and 


/'(x) > 0. 


□ 


To summarize, using techniques from geometry and Green’s theorem 
one can generate strategies for rolling the planar surface of an object 
face to cause certain desired motion in the spherical fingertip and the 
angle of contact. The method appears to be ad hoc and specific to the 
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specific geometry of the finger and the object, however, it may in fact 
be generalized to other geometries. In fact, there is a generalization of 
the method to situations other than fingers rolling on objects called the 
method of geometric phase. For example, this method can be used to solve 
problems of reorienting satellites in space or to give an explanation for 
how cyclic motions of cilia on the surface of a paramecium cause forward 
motion in the paramecium. 
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5 Summary 

The following are the key concepts covered in this chapter: 

1. Optimal controls (minimizing integral least squares cost) for steer¬ 
ing a system with growth vector (2, 3) of the form 

<7i = ui 
<72 = u 2 

q3 = qiU2 - q2Ui 

are sinusoidal. Further more when <71 (0) = ( 72 ( 0 ) = <71 (1) = < 72 ( 1 ) = 
0 , the optimal inputs are sinusoids at frequency 27 t. 

2. For a system of the form 

q = u 

Y = qu T — uq T 

the optimal steering inputs, minimizing the integral least squares 
cost, are sinusoidal. Further, when q(0) = g(l) = 0 the optimal 
inputs are sinusoids at integrally related frequencies. 

3. Using integrally related sinusoids as (sub-optimal) inputs, one can 
steer chained form systems. A one-chain system is one of the form 

<7i = mi 
q-i = u 2 

<73 = 92^1 
<74 = <73Ml 

Qn = Qn— 1^1 

Generalizations to multi-chain systems also exist. Involutivity con¬ 
ditions for converting given control systems into the chained form 
may be given. 

4. While it is difficult to give closed form expressions for the optimal 
controls associated with solving the least squares steering problem 
for a nonholonomic control system, one can derive formulas for the 
time derivatives of the optimal inputs. Further, numerical tech¬ 
niques, such as the Ritz approximation algorithm, may be used to 
derive approximate algorithms for generating the optimal controls. 

5. Piecewise constant inputs can be used to steer a nonholonomic con¬ 
trol system in the Philip Hall basis coordinates when the controlla¬ 
bility Lie algebra is nilpotent. 


389 


6 . Dynamic finger repositioning on the surface of an object can be 
carried out using sinusoids. In the special case of a spherical finger 
rolling on the surface of a flat object, the geometry of the Gauss- 
Bonnet theorem may be used to position the finger on the object 
surface and adjust the angle of contact. 

6 Bibliography 

While research in nonholonomic behavior of mechanical systems is quite 
classical, interest in steering and trajectory generation is quite recent. To 
our knowledge, the connection between nonholonomy and constructive 
controllability was first pointed by Laumond [55] in the context of mobile 
robots and Li [58, 60] in the context of fingers rolling on the surface of 
a grasped object. The literature in controllability for nonlinear systems 
is quite extensive. A good review of it is to be found in the textbooks 
of Nijmeijer and van der Schaft [83] and Isidori [43]. However, some of 
the most important first results on constructive controllability appeared 
in [11] and [4], where the least squares steering problem was solved for 
a class of model systems on R n and SO( 3) respectively. 

In this chapter, we have used as source material some of our own recent 
publications in this area, such as [60] and [77] for steering fingers rolling 
on the surface of an object, [78] which discusses the use of sinusoids in 
steering nonholonomic systems, [103] which discusses the structure of op¬ 
timal controls for steering problems, [32, 33] on the Ritz approximation 
procedure for solving optimal control problems, and [112] which solves 
the problem of parking a car with N trailers. A recent collection of pa¬ 
pers [61] contains a good cross-section of papers on nonholonomic motion 
planning for further reading. A discussion of nonholonomic mechanics 
and geometric phase is in [67, 68]. 
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7 Exercises 

1. Show that the following system 

91 = Wi 

92 = U 2 

93 = 91^2 

1 2 

94 = 2 91 U 2 

95 = 9192^2 

1 3 

96 = g9i u i 

1 2 

97 = 29l92M2 

1 2 

98 = ^QlQ 2 U 2 

is controllable and nilpotent of degree four. Can you find a nonlinear 
change of coordinates to transform this system into a one-chained 
form? 

2. Show that the following system is controllable and nilpotent: 


91 = Mi 

92 = u 2 

93 = 9lM2 ^ 92«1 

94 = 91^2 

95 = 92 u 1- 

3. Consider the following control system 

9 = u 

Y = qu T — uq T 
where u £ R m and Y £ so(m). 

(a) Derive the Euler-Lagrange equations for the system, by mini¬ 
mizing the following integral 



(b) For the boundary conditions 9 ( 0 ) = 9 ( 1 ) = 0, Y(0) = 0 and 
Y(l) = y for some y £ M 3 , solve the Euler Lagrange equations 
to obtain the optimal inputs u. 
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(c) Find the input u to steer the system from (0,0) to (0, Y) £ 
K m x so(m). 

4. Consider the following system 


91 = Mi 

92 = u 2 
912 = 91^2 

9121 = 912^1 

9122 = 912^2 


Apply the inputs 

Mi = ai sin 2-rrt + a 2 cos 2nt + a 3 sin 47 rf + 04 cos 47 rt 
u 2 = bi sin 2nt + b 2 cos 2irt + 63 sin 47 rf + 64 cos 4nt 

to this system and integrate 9121 and 9122 from t = 0 to t = 1 
to obtain a system of polynominal equations in ( a,i,bi ). Propose a 
method for solving for the coefficients ( at,bi ) given the initial and 
final states. 

5. Two-input two-chained system 

The following system is referred to as a two-input, two-chained 
system 

x 0 = Ml Z/o = U2 

Xi = Z/ 0 W 1 iyi = XqU 2 ) 

•En x — %n x — 1^1 Vny — Vriy — 1^2 

where y\ := Xoyo — x\ to account for skew-symmetry of the Lie 
bracket. 

(a) Prove that the system is controllable. 

(b) Show that for each qk,k > 1, the inputs u\ = asin2nt and 
u 2 = b cos 2irkt steer the system to the final value of qk ■ Give 
an explicit formula for the final value of qk in terms of ( a,b ). 

(c) Show that for each yk,k > 2, the inputs Mi = 6cos27rfcf and 
u 2 = asin27rt steers the system to the final value of yk- 

(d) Give an algorithm for steering the system from an initial state 
to a final state. 

6 . In the proof of Proposition 8.4, it was asserted that if u satisfies the 
differential equation 

m = Vl(t)u 
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for some 12 £ so(to), then the solution of u is of the form 

u(t) = U (t)u(O) 

for some U(t) £ SO(m). Prove this assertion. 

7. Proposition 8.4 gave a formula for the first derivative of the optimal 
inputs. Use the method of the proof of that proposition to obtain 
a formula for ii. Express the answer in terms of Lie brackets of the 
input vector fields. In fact, if you are adventurous, try to find the 
formula for u^. 

8 . Use the results of Proposition 8.16 to show that the optimal input 
of that proposition normalized by ||u( 0 )||, that is, 

u{t) 

IH0)ll 

solves the minimum time steering problem to steer the system from 
< 7 ( 0 ) = qo to q(T) = qf subject to the constraint that ||zt(i )|| 2 < 1 
for all t. 

9. Apply the methods of Proposition 8.16 to solve the optimal control 
inputs for the model control system we studied in Section 2, namely 

<7i = ui 
<72 = u 2 

<73 = - q 2 u 1 . 

10. Extend the method used to find the optimal inputs for Engel’s 
system to find optimal inputs for the system of Exercise 2. 

11. Consider the least squares optimal input steering problem for a 
system with drift: 


<7 = f(q) + ^ 2 gz{q)ui. 


(a) Find the expression for the optimal Hamiltonian and prove 
that the optimal inputs satisfy the differential equation 


= n(q,p)u = 


p T [f , 91 ] 


P T [f, 9-n 


(8.38) 


with tt(q,p) defined as in (8.16). 
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(b) Find the second derivatives of the optimal inputs given in 
equation (8.38). 

12. Consider the following system 

91 = ui 

92 = u 2 

93 = 92«1 

with initial condition g(0) = 0. Let the inputs be of the form 

U\ = aqoai.i cos 27 t t + o.\ t2 sin 27 t t 
u 2 = a 2} o + « 2 ,i cos 27r£ + « 2,2 sin 27rf. 

(a) Integrate the control system symbolically from t = 0 to t = 1 

to obtain g(l) as a function of a, the coefficient vector of the 
inputs, and compute the Jacobian A = € R 3x6 . 

(b) Prove that if A is full rank then the system is controllable with 
inputs of the above form. 

13. Consider the one-chain system in equation (8.7). For the Philip 
Hall basis g\, g 2 , ad^ g 2 , k = l,... ,n — 1, derive the Chen-Fliess- 
Sussmann equation. 

14. Consider the following system 


<7i = ui 

q2 = u 2 

93 = 91^2 - 92«1 

94 = q\u 2 . 

(a) Derive the Chen-Fliess-Sussmann equation. 

(b) Assuming that the inputs are of the form 

u\ = ag + a i cos 27 it + a 2 sin 27rt 
u 2 = b 0 + bi cos 27 t t + b 2 sin 2nt, 

compute the polynomial equation for the amplitude parame¬ 
ters in terms of the initial and final states. 
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Chapter 9 

Future Prospects 


In this book, we have tried to give the reader a feel for the sorts of analyt¬ 
ical tools that one needs in the study of robotic manipulation. We have 
adopted a mathematical point of view because of its compactness. One 
thing that this mathematical point of view masks is the excitement that 
we feel for robotics technology and its future because of the considerable 
innovation and the development being made in its use. The robotics in¬ 
dustry has reached one plateau with the successful introduction of robots 
into automotive manufacturing—spot welding and painting are two are¬ 
nas where robotic usage is almost universal, assembly of engines is an 
area where the amount of utilization is more varied—and into electronic 
assembly. There are several other areas where the usage of robotics is in 
its infancy and this chapter is dedicated to brief descriptions of some of 
these fields along with a quick assessment of their current status. 

One question that comes up often in such a retrospective is the differ¬ 
ence between “teleoperation” and “robotics.” While precise definitions 
and distinctions between these two topics are elusive, the rough distinc¬ 
tion appears to be in the amount of human interaction: “human intelli¬ 
gence” rather than “machine intelligence” required for the operation of 
the same set of basic devices. Consequently, it hardly seems surprising to 
us that in the natural course of evolution of the technology, teleoperation 
will precede true robotic or autonomous operation. Indeed, the pragmatic 
point of view would be to favor the introduction of new robotic devices 
first in teleoperated form. This has also historically been the path of 
evolution of the held. Nonetheless, there are good reasons for not having 
a person in the feedback loop in many applications: 

1. Communication delays in transmission of sensor information and 
receipt of command information 

2. Slow speed of response of humans to numerical and quantitative 
data 
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In our opinion, the teleoperators of today are the autonomous robots of 
tomorrow and as such we will not make a distinction between them in 
this chapter. 

One other topic that comes up a great deal in the popular science 
press is the concurrent usage of the terms virtual reality , and telepres¬ 
ence. The term virtual reality refers to the remote creation of a synthetic 
environment containing sights, sounds, touch, and forces. While remote 
sight and sound are easily implemented, remote touch and force are not 
as easily achievable. In order to manipulate objects remotely (telepres¬ 
ence), it is important to be able to have each of these senses. Thus, there 
is an intimate relationship between remote manipulation or telepresence 
and virtual reality. Indeed, we see that the bulk of the current litera¬ 
ture on virtual reality is really about telepresence, since the purpose of 
simulating a remote environment is to allow a person to interact with it. 

In summary, we feel that the technological progress in the years to 
come will be on a broad front spanning teleoperation, virtual reality, and 
dextrous manipulation. In the rest of the chapter, we will say a little 
bit about the opportunities. The sections are organized according to 
the scale of the robots: Section 1 deals with conventionally sized robots, 
Section 2 with robots at the millimeter scale, and Section 3 with robots 
at the micrometer and nanometer scale. 


1 Robots in Hazardous Environments 

One of the chief areas for the future (and current) use of robots of the 
conventional size is in hazardous environments. In this section, we give 
a brief description of the sorts of environments in which robots will be 
(and are) found. 


Space 

The best known example of a robot in space is the 20 meter long re¬ 
mote manipulator system on board the space shuttle. It has six degrees 
of freedom and is usually manually teleoperated by an astronaut under 
direct visual feedback. There is, however, the ability to have the robot 
be moved under computer control in Cartesian coordinates. There are 
Japanese plans to build a flight telcrobotic servicer which has two cooper¬ 
ating robot arms for repairing satellites and other coordinated activities 
on board a self-propelled platform. Other examples of robots in space 
include the Mars rover and other planetary exploring robots which fea¬ 
ture tracked or wheeled mobile bases with arms on board for scooping 
soil samples. It is not anticipated that these devices will be under human 
control remotely since the transit time delays for commands are too high 
to allow for meaningful remote feedback actions. 
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Underwater 

In the last decade, several remotely-operated vehicles have been built 
for inspection of underwater oil derricks and exploration. Their devel¬ 
opment has been motivated by the high cost of human divers and the 
risk to life of working under water. For the most part they consist of a 
mobile platform, either on an umbilical line from the mother ship or com¬ 
pletely autonomous, fitted with one or more robotic arms. Most of these 
robots are remotely piloted and most current undersea manipulators are 
hydraulic to withstand the high forces and corrosive elements that they 
need to withstand. In the future, there will be a surge in the number 
of completely autonomous robots for exploration of the ocean floor and 
other unstructured environments for which the human reaction time is 
too large. 

Nuclear, toxic waste disposal and mining 

Some of the earliest work in robotics came from teleoperators for handling 
radioactive material. In recent years mobile robots with robotic arms 
onboard for inspection, maintenance, and even for handling of spent fuel 
rods, have become more prevalent. With the growth in the extent and 
nature of hazardous materials that need to be disposed worldwide, robots 
for handling and disposing toxic materials will need to be developed. 
Mining environments are similarly hazardous and there are already quite 
a few different kinds of mining vehicles and arms that can be remotely 
operated. 

Firefighting, construction and agriculture 

One can visualize a scenario in which maps of buildings would be down¬ 
loaded onto robots at the scene of a fire. These robots can then be 
used for firefighting using onboard heat and smoke detectors and trailing 
an umbilical cord carrying water as well as fire-retardant chemicals and 
relaying video data to remote locations. In this application, it seems im¬ 
portant to have robots that can negotiate stairways as well as corridors 
(i.e., legged as well as wheeled robots). Automated construction is a field 
in its infancy, but robotic tools for accurate and risk-free construction, 
sometimes in underground or underwater environments, are developing 
rapidly. In agriculture, multifingered robot hands mounted on an arm 
and equipped with vision systems have been used for picking oranges 
and for harvesting crops. It is thought that robots could also be used for 
tilling and planting. 

Robotic systems have also been used for deboning meat in meat pack¬ 
ing plants in Australia. Finally, an amazing robotic system that has been 
used for sheep shearing has been developed at the University of West Aus- 
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tralia and features the development of a wrist with no singularities in the 
workspace of the manipulator [114]. 


2 Medical Applications for Multifingered Hands 

In recent years, there has been a great deal of excitement about min¬ 
imally invasive surgery, including a number of techniques for accessing 
internal organs through small incisions or orifices in the body (varying in 
size from about 3 millimeters to 11 millimeters). As a result, trauma to 
muscles and other tissue which need to be cut in traditional surgery is 
minimized, resulting in a considerable savings in recovery time, risk to life 
during the operation and hospital stay. Typically, in these procedures, 
slender probes are introduced via a puncture and tools such as probes 
with laser light sources, cameras, and instruments are fed into the body 
cavity. In some cases, the body cavity is distended with gas (usually the 
abdominal cavity) to create viewing room. The advances in active optics 
(CCD imagers and high resolution displays) and fiber optics have made it 
possible for the surgeon to have very high quality images of the inside of 
the body through a small aperture. Several instruments can then be used 
to take advantage of this vision of the inside of the body cavity: the endo¬ 
scope is used for the inside of the gastro-intestinal tract, the laparoscope 
for the abdominal cavity, the thoracoscope for the thoracic cavity and 
the arthroscope for the inside of the joints. Several procedures, such as 
the removal of the gall bladder (cholecsystectomy) using the laparoscope, 
removal for biopsy of polyps in the gastro-intestinal tract using the en¬ 
doscope, repairing hernias in the lung cavity using the thoracoscope, and 
scraping away scar tissue in the knee joint using the arthroscope, are now 
commonplace. Of the 600,000 cholecsystectomies performed annually in 
the U. S. it is thought that up to 500,000 are performed laparoscopically 
and, according to some practitioners, minimally invasive techniques will 
dominate “open” surgery in the future. 

However, what limits minimally invasive surgery is manipulator tech¬ 
nology, for the following reasons: 

1 . Inadequate degrees of freedom. Current needle holders, cutters and 
other tools transmit a surgeon’s hand motion through passive mech¬ 
anisms. Further, tactile feedback is disrupted. Foremost among the 
limitations imposed by today’s tools is their limited number of de¬ 
grees of freedom. For example, a needle driver that can slide, twist 
and pivot (up and down as well as left and right) inside the body 
cavity gives a surgeon only four degrees of freedom without full 
control of orientation. Thus, suture lines must radiate from the 
insertion point, since the needle can only be driven by twisting the 
driver about its long axis. 
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2. The need for fine motion control in surgery. In open surgery the sur¬ 
geon braces herself so as to reduce the amount of tremor transmit¬ 
ted to the end of the surgical device. In the instance of minimally- 
invasive surgery, the fulcrum at the point of entry of the instrument 
reduces the tremor for pivoting motions of the tool, but does not 
help positioning accuracy in the other directions caused by shoulder 
and elbow tremor. 

Improved manipulators with many degrees of freedom would increase 
efficiency, safety, and the range of cases that could utilize these methods 
by addressing the drawbacks mentioned above. However, the kinematics 
of useful devices is complex for many reasons. To realize the gains of min¬ 
imally invasive surgery, we feel that the technology of multifingered robot 
hands could be brought to bear. The design, construction, and control 
of a miniature hand-like manipulator requires significant departures from 
more traditional robot manipulators. Because of the small sizes of the 
fingers (on the order of millimeters), direct actuation of each rotary or 
prismatic joint is not practical. In some of our own preliminary work, we 
have constructed small fingers which are either controlled by cables or by 
small hydraulic actuators. The fabrication techniques for the manipula¬ 
tor are borrowed from integrated circuit technology. Although common 
metal and plastic materials are capable of developing biologically signifi¬ 
cant forces at this small scale, it is a challenge to develop actuators which 
exhibit large enough ranges of motion. 

At the outset, teleoperator technology which is used to reflect the 
actions of the surgeon into the body cavity will be used to control the 
surgical manipulators. User interfaces such as sensor gloves worn by 
the surgeon would provide the surgeon with tactile and force feedback, 
while the positioning of the fingers would be measured by sensors and 
transmitted to small multifingered hands. One such system has been 
proposed by us in [19]. 

The growth potential of this application is enormous. Remote surgery 
is being explored for use on the battlefield and in space, and with greater 
intelligence, control, and sensing built into surgical manipulators, one can 
conceive of surgical workstations in the not too distant future. 


3 Robots on a Small Scale: Microrobotics 

In many new applications, it is necessary to handle or manipulate very 
small objects, for example living cells or parts of semiconductor electron¬ 
ics. The scale of operations that we visualize in these applications are 
several orders of magnitude smaller than those involved in the surgical 
applications of Section 2, which we have termed milli-robots. Thus, in 
this section we will concentrate on micro- and nano-scale robots. There 
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are two different approaches to dealing with these small objects: the first 
to use a conventional (large) manipulator with a very precise control sys¬ 
tem and the second is to miniaturize the manipulator. There are many 
advantages to shrinking the robots to the same scale as the parts being 
manipulated: 

1. Delicate forces can be applied. 

2. Robots can be made more accurate. 

3. Robots can be fabricated using silicon processing and photolitho¬ 
graphic techniques. 

The notion of a micro-robot on a chip has been popularized by Brooks and 
Flynn [16] and Pister, Fearing, and co-workers [91, 92, 93]. We foresee a 
scenario in which these robots see wide application in micro-teleoperation 
in cramped areas, and in massively parallel handling of small biological 
and electromechanical systems. In this section, we abstract from Fear¬ 
ing [31] some of the technological challenges and opportunities in this 
rapidly growing area. 

There are many engineering issues to be addressed in building micro- 
robotic systems: the power source, the propulsion method (if they are 
mobile), control integrated with sensing, and communications with the 
macro-world. One key new technology that provides new capabilities for 
sensing and actuation at the micro scale is micromachining. This is the 
ability to machine at very small scales, including the micron scale, us¬ 
ing techniques from integrated circuit fabrication. This can be used, for 
instance, to produce actuators which have hundreds of miniature cilia 
(like a paramecium); or to make mechanisms like grippers that can han¬ 
dle parts of the size of 10/r diameter or planar rotary motors of a few 
microns size and sensors like miniature gyroscopes. At these sizes, forces 
scale differently so that electrostatic forces are stronger than electromag¬ 
netic forces. This necessitates a rethinking of actuation methods for these 
mechanisms. 

Intelligent sensors, actuators and control systems can be integrated on 
a single chip. A novel integrated system for manipulating dry parts in the 
plane was proposed by Pister et al. [92]. This system consists of a 1 cm 2 
silicon substrate with an air bearing to support individual 1 nun 2 plat¬ 
forms. The individual platforms are driven in the plane by electrostatic 
forces and can carry grippers, probes for sensing, or tools for processing. 
Capacitive position sensing of the platforms is added to complete the sys¬ 
tem. This system has been partially fabricated and a conceptualization 
of it is presented in Figure 9.1. It was designed in analogy to macro-robot 
manipulator called Robotworld (made by Automatix Corporation) and 
was made to automatically align and splice together fiber optic cables. 

Current micro-mechanical systems are for the most part planar. How¬ 
ever, it is clear that for manipulators to extend far beyond the surface 
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Figure 9.1: (a) View of several probing platforms floating on the same 
bearing surface, (b) Detailed view of a single platform. (Figures courtesy 
of Kristofer Pister and Ronald Fearing) 


that they are mounted on, it is important to have 3-D structures. A 
promising new approach in this regard is the micro-hinge method of Pis¬ 
ter et al. [93]. This approach consists of actually fabricating components 
in 2-D, but providing them with the ability to rotate or slide into place 
resulting in the assembly of a 3-D structure. By using techniques drawn 
from origami, the Japanese art of paper folding, the structures can be 
made to self assemble under agitation in a water bath after emergence 
from the silicon foundry. 

In the years to come we feel that there will be an explosive growth 
of micro-machined robots with propulsion capabilities in fluids and with 
onboard robots and multifingered robots for manipulation so as to do a 
variety of tasks both on the biological and integrated circuit fronts. 
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Appendix A 

Lie Groups and Robot 
Kinematics 


Readers familiar with differential geometry will have observed that most 
of the analytic tools we use in this text are derived from Lie group theory 
and Riemannian geometry. In this appendix, we give a brief introduction 
to the basics of Lie group theory and its connections with rigid body 
kinematics. 


1 Differentiable Manifolds 

We begin with a brief review of differential geometry, based on the treat¬ 
ment given by Boothby [9]. The material here is intended primarily to 
fix the notation used in the subsequent sections. 

1.1 Manifolds and maps 

Let U C R” and V C R m be open sets. A mapping /:[/—> V is a smooth 
map if all partial derivatives of /, of any order, exist and are continuous. 
If m = n and / is bijective and both / and / _1 are smooth, then / is 
called a diffeomorphism and U and V are said to be diffeomorphic. 

A manifold of dimension n is a set M which is locally homeomorphic 
to R ra . We parameterize the manifold by using a set of local coordinate 
charts. A local coordinate chart is a pair (<j>,U), where (f> is a function 
which maps points in the set U C M to an open subset of R™. Two 
overlapping charts (</>, U ) and (if, V ) are C°° related if if- 1 o cf is a dif¬ 
feomorphism where it is defined. A collection of such charts with the 
additional property that the C/’s cover M is called a smooth atlas. A 
manifold M is a smooth manifold if it admits a smooth atlas. 
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The properties of mappings between manifolds are defined in terms 
of local coordinate charts. Let F : M —> N be a mapping between 
two smooth manifolds and let (U, tf) and (V, ip) be coordinate charts for 
M and N, respectively. The mapping F : M —> ./V is smooth if F = 
ip o F o p - 1 : (p{U) —> ip(V) is smooth for all choices of coordinate charts 
on M and N. Similarly, F is a diffeomorphism if F is a diffeomorphism 
for all coordinate charts. 


1.2 Tangent spaces and tangent maps 

Let M be a smooth manifold of dimension n and let p be a point in 
M. We write C°°(p ) for the set of smooth, real-valued functions on M 
whose domain of definition includes some open neighborhood of p. A 
map X p : C°°(p) —► R is called a derivation if, for all a, (3 £ R and 
/, g € C°°(p), it satisfies 

(i) X p (af + /3 g) = a(X p f) + j3{X p g ) (linearity) 

(ii) Xp(fg) = (Xpf)g(p) + f(p)(X p g) (Leibniz rule) 

The set of all derivations X p : C°°{p ) —> R defines a vector space over 
the reals with the operations 


(X p + Y p )f = X p f + Y p f 
(aX p )f = a(X p f). 


The tangent space of M at a point p , denoted T p M, is the set of 
all derivations X p : C°°(p) —> R. Elements of the tangent space are 
called tangent vectors. Let (U, <p) be a coordinate chart on M with local 
coordinates (xi,..., x n ). Then, the set of derivations { ^-} forms a basis 
for TpM and hence we can write 


A p 



+ • • • + X n 


d 

0x n 


The vector (Xi,... ,X n ) £ R” is a local coordinate representation of 
X p £ T p M. 

Let F : M —> N be a smooth map. We define the tangent map of F 
at p as the linear map F* p : T p M —> T F ( p \N defined by 


F*pX p (f) = X p (f o F), 


where X p £ T p M and / £ C°°(F(p)). We also make use of the notation 
T p F to denote the tangent map of F at p. The tangent map satisfies the 
following properties: 

1. If H = FoG is the composition of two smooth maps F and G, then 
H* p = F* G (p) ° G* p or T p H = T G (p)F o T p G. 
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2. If F : M —> N is a diffeomorphism, then : T p M —► Tp^N is an 
isomorphism of tangent spaces with inverse (-F* p ) -1 = (F~ 1 )*p( p y 

If c : R —> M is a curve in M such that c(0) = p and c(0) := c*t( Jj)|t=o = 
X p , then T p F(X p ) = f t F{c{t ))| t=0 . 

Let M be a manifold of dimension n. The tangent bundle of M, 
denoted TM, is a manifold of dimension 2 n defined by 

TM = (J T p M. 

p£M 

An element of TM will be written as (p, X p ) or simply X p , where p £ M 
and X p £ T p M. There is a natural projection tt : TM M given by 
n(X p ) = p. 


1.3 Cotangent spaces and cotangent maps 

Given the tangent space T p M to a manifold M at a point p, we define the 
cotangent space of M at p , denoted T*M , as the set of all linear functions 
uip : T p M —> R. T*M is a vector space having the same dimension 
as TpM and elements of T*M are called cotangent vectors. We write 
(ui p ,X p ) for the action of a cotangent vector oj p £ T*M on a tangent 
vector X p £ T p M. If ..., gf-} is a basis for T p M corresponding 

to local coordinates (x±,... ,x n ), the dual basis for T*M is given by 
{dx i,..., dx n }, where 


(dx i: 



= 6 . 


ij, 


i,j = 1 


Given a function / : M —> R, we define a cotangent vector df(p) £ 

t;m by 

(df(p),X p )=X p (f), Xp £ TpM. 

df(p) is called the differential of f. Relative to a chart (<fi, U) with local 
coordinates x = (cci,..., x n ), df(p) is written as 

df{x) = ^-{x)dx i H-h ^-(x)dx n , 

C 'Jbf (JJb'Y'i 


where x = </>(p). 

Let F : M —> N be a smooth map. The cotangent map of F is the 
linear map F* : Tf^N —> T*M defined by 

{F p O i F(p )) Xp) = (ftjr(p), F^pXp ), ttF(p) £ Tp^p^N, Xp £ TpM. 

We also make use of the notation TfF to denote the cotangent map of 
F at p. 
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Let M be a manifold of dimension n. The cotangent bundle of a 
manifold M, denoted T*M is a manifold of dimension 2 n defined as 

T*M = (J T*M. 

pGM 

An element of T*M will be written as (p,a p ) or simply a p £ T*M. 


1.4 Vector fields 

A smooth vector field A on a manifold M is defined as a smooth map 
X : M —> TM satisfying noX = id, where tt : TM —> M is the canonical 
projection and id : M —> M is the identity map on M. We let %(M) 
denote the set of all smooth vector fields on M. Relative to a coordinate 
chart U), a vector held is written as 

o o 

X(x) = Xi(x)— -1- \-X n (x )~—, 

where each A,; is a smooth function defined on an open neighborhood of 
x = (j>(j>). It is customary to write a vector held as a column vector 


X(x) 


Xi (x) 
X n {x) 


Vector helds represent differential equations on manifolds. Let c : 
(a, b) —> M be a curve on the manifold. The curve c is said to be an 
integral curve of the vector held X if 


c{t) = X{c{t)). 


By the existence and uniqueness theorem for ordinary differential equa¬ 
tions, the existence of integral curves for a given nonzero vector held is 
guaranteed locally. The vector held is said to be complete if the domain 
of definition of the integral curves can be chosen to be (— 00 , 00 ). In this 
case, the integral curves of a vector held define a one-parameter family 
of diffeomorphisms ^(g) : M-t M with the understanding that $ 4 ( 9 ) is 
the point on the integral curve starting from initial condition q at t = 0 . 
This one parameter family of diffeomorphisms is referred to as the flow 
of the vector held X. 

Let A be a smooth vector held and / £ C°°(M) a smooth function 
on M. The Lie derivative of / with respect to A is a new function 
A/ : M —i- M dehned by 

A f(p) = X p f. 
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In coordinate chart (<j), U), if we write X = ]T" =1 Xi(x) g§r, then 
*/(*) = 

i— 1 

where all partial derivatives are evaluated at x = 4>{p). 

Let X and Y be two smooth vector fields. The Lie bracket of X and 
Y, denoted [X, Y], is a new vector field defined by 

[X,Y\f = X(Yf)-Y(Xf). 

It is not difficult to show that [A, Y] satisfies all the properties of a 
derivation. In local coordinates, if we write X = Xi(q) and 

Y = d x ■ then the Lie bracket vector field [ X , Y] is given by 


[v,n = £ 

j'=i 




d 

dxj' 


Let F : M —> TV be a smooth mapping between manifolds and X £ 
X(M), Y £ X(N) smooth vector fields. We say that X and Y are F- 
related if they satisfy 

^ f ( p ) = F*pXp. 

If F is a diffeomorphism, then given a vector field X £ X(M) we can 
define a new vector field Y £ X(N) via the push forward map F* : TM —> 
TN defined by 

(F*X) q = F tF -i( q )X F -i( q y 

Similarly, if F : M —> N is a diffeomorphism and Y is a vector field on 
N, we can define a new vector field on M as 

(F*Y) p = (F* p )- 1 Y f{p) . 

The mapping F* : TN —» TM is called the pull back map for F. The 
pull back is related to the push forward by the formula F* = (F^ 1 )*. 

Proposition A.l. Let X and Y be smooth vector fields on M and F : 
M —> N be a smooth map. Then 


F4X,Y} = [F.X,F.Y], 

A vector space V (over R) is a Lie algebra if there exists a bilinear 
operator V x V —> V, denoted [-, •], satisfying 

1. Skew-symmetry: [v, ic] = —\w,v\ for all v,w £ V 
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2. Jacobi identity: 


[[u,w)],z] + [[z,v\,w\ + [[w,z],v\ = 0 
for all v ,w,z £ V 

A subspace W C V is called a Lie subalgebra if [v, w\ £W for all v, w £ 
W. The vector space of all smooth vector fields on a manifold M is an 
infinite-dimensional Lie algebra under the Lie bracket operation on vector 
fields. 

1.5 Differential forms 

A smooth differential one-form , on a manifold M is a smooth map a : 
M —> T* M satisfying n o a = id where n : T*M —> M is the canonical 
projection and id : M —* M is the identity map. In local coordinates, a 
differential form is written as 

a(x) = ai{x)dx\ + • • • + a n (x)dx n 

where each on is a smooth function on M. 

Let F : M — » N be a smooth mapping between manifolds and (3 : 
N —> T*N a smooth differential one-form on N. We can define a new 
one-form a : M —> T*M by 


(Ap, Ap) if F(p) • b\ : p-Xp). 

We call a the pull back of /3 by F and write a = F*(3. Note that the pull 
back of a one-form is defined for any smooth mapping F : M —> TV, not 
just diffeomorphisms. 


2 Lie Groups 

This section collects some basic concepts of Lie groups which prove to 
be useful in robot kinematics and control. A more detailed treatment of 
these subjects can be found in Spivak [108]. Explicit formulas for SO( 3) 
and SE( 3) are given by Park and Murray [88]. 

2.1 Definition and examples 

A Lie group is a group G which is also a smooth manifold and for which 
the group operations ( g , h) i—> gh and g i—> g~ x are smooth. A Lie group 
is abelian if gh = hg for all g, h £ G. We will use the symbol e to denote 
the identity element of the group. 

For every g £ G, we define left translation by g as the map L g : G —> G 
given by L g (h) = gh for h £ G. Similarly, right translation by g is defined 
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as the map R g : G —> G satisfying R g (h) = hg. Since L g o Lh = L g h and 
R g ° Rh = Rgh , we have that ( L g ) _1 = L g - 1 and ( R g ) _1 = R g - 1 . Thus, 
both L g and R g are diffeomorphisms of G for each g. Moreover, left and 
right translation commute: L g o R h = R h o L g . If the group is abelian 
then L g = R g . 

Example A.l. The Euclidean space under addition 

The Euclidean space R n with group operation (x,y) i—> x + y is an abelian 
Lie group. The inverse of a; £ R™ is denoted —x, and the identity element 
is the zero vector. Since x + y = y + x, such a Lie group is abelian. 

Example A.2. The general linear group, GL(n,R) 

The group of all n x n nonsingular real matrices is called the general linear 
group and denoted GL(n,M). As a manifold, GL(n,R) can be regarded 
as an open subset of R" . For A,Be GL(n, R), the group operation is 
matrix multiplication 

(A, B) t—> A ■ B 

and inversion is given by the matrix inverse. Both operations are smooth 
since the formulas for the product and inverse of matrices are smooth in 
the matrix components. The identity element is the nxn identity matrix. 
Left and right translation are defined as left and right multiplication, 
respectively. 

Example A.3. The special orthogonal group, SO(n ) 

The special orthogonal group is a subgroup of the general linear group, 
defined as 


SO(n) = {Re GL{n, R) : RR T = I,detR = +1}. 

The dimension of SO(n) as a manifold is n{n — l)/2. For n = 3, the 
group SO( 3) is also referred to as the rotation group on R 3 . 

Example A.4. The special Euclidean group, SE(3) 

The group of rigid transformations on R 3 is defined as the set of mappings 
g : R 3 —> R 3 of the form g{x) = Rx + p, where R € 50(3) and p £ R 3 . 
An element of SE( 3) is written as (p, R) £ SE( 3). SE{ 3) can be identified 
with the space of 4 x 4 matrices of the form 


where R £ 50(3) and p £ R 3 . SE{ 3) is a Lie group of dimension 6. 

2.2 The Lie algebra associated with a Lie group 

Let A be a vector field on G. X is left invariant if {L g ) if X = X , that is 
ThLgX(h) = X(gh) for all h £ G. 
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Let Xl(G) be the set of left invariant vector fields on G. Then, for 
X, Y £ Xl{G) we have 

Lg.[X,Y\ = [L g *X,L g *Y] = [X,Y]. 

Thus, Xl(G) is a Lie subalgebra of the Lie algebra X(G), the set of all 
vector fields on G. 

For each £ £ T e G , we define a vector field X £ on G by 

X i (g)=T e L g Z. 

Since 

X^gh) = T e L gh ■ £ = T e (L g o L h ) • £ 

= T h L g {T e L h • £) = T h L g (Xs(h)), 

X £ is left invariant. The linear maps p\ : 3 Ll{G) —► T e G given by 

Pl (X)=X(e) 

and p 2 : T e G —► Xl{G) given by 

p 2 (£)=^ 

satisfy p\o p 2 = idj- e G and p 2 ° Pi = idx L (C)- Hence Xl(G) and T e G are 
isomorphic as vector spaces. Defining a Lie bracket in T e G by 

[£i,6] = [X ii: X i2 }(e), £ 1; £ 2 £ T e G (A.l) 

makes T e G into a Lie algebra. The vector space T e G with this Lie alge¬ 
braic structure is called the Lie algebra of G and is denoted g. 

A Lie subalgebra of g is a subspace f) C g such that £, r) £ 1) implies 
that [£, Tj] £ 1). It can be shown that if H is a Lie subgroup of G with Lie 
algebra f), then f) is a Lie subalgebra of g. 

Example A.5. The Lie algebra of (R n ,-|-) 

For the group R" we have e = 0, ToR” = R”, and it is easy to see that 
the left invariant vector field defined by v £ ToR n is the constant vector 
field: X v (x ) = v for all x £ R". Therefore, the Lie algebra of R ra is R” 
itself, with the trivial Lie bracket [ui,!^] = 0 for all vi,v 2 £ R ra - 

Example A.6. The Lie algebra of GL(n,R) 

The Lie algebra of GL(n, R) is the set of all n x n real matrices, denoted 
g[(n,R), with the bracket structure 

[A,B]=AB-BA A,B£ gl(n,R). 

To derive this, note that GL(n, R) is an open subset of R raxn and hence 
T e GL(n, R) = R” xn . A vector field on GL(n, R) can be written as 

X(x) Xij(x)-g^~- 

i,j 
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where each Xij represents a coordinate in R” xn . One can now proceed 
to define left-invariant vector fields on R nxn (relative to left matrix mul¬ 
tiplication) and compute the Lie bracket from equation (A.l). See [108, 
pp. 509-511] for the details of this calculation. 


Example A.7. The Lie algebra of S'0(3) 

The Lie algebra of SO( 3), denoted so(3), may be identified with the 3x3 
skew-symmetric matrices of the form 


0 

-013 

u>2 

013 

0 

-Oil 

—012 

Oil 

0 


(A.2) 


with the bracket structure 


[cDi, cD 2 ] = S1O2 - W2W1, 


£ 1 , 0)2 £ so(3). 


We can identify so(3) with R 3 using the mapping in equation (A.2), which 
maps a vector 01 £ R 3 to a matrix £ £ so(3). It is straight forward to 
show that 

[001,012] = (oil x 012)^, oil, 012 £ R • 

Thus 011 —> £ is a Lie algebra isomorphism between the Lie algebra (R 3 , x) 
and the Lie algebra (so(3), [-,•]). 


Example A.8. The Lie algebra of SE(3) 

The Lie algebra of SE( 3), denoted se(3), can be identified with 4x4 
matrices of the form 


£ = 


01 v 

0 0 


o;, v £ 


with the bracket structure [^ 1 ,^ 2 ] = £i £2 — £ 2 £i- Let 


6 = 


u>i Vi 

0 0 


and £2 = 


oi 2 u 2 
0 0 


Then 


[ 6 . >£2] = £1^2 - £2^1 = 


(oil X W2) A 011 X V2 — 012 x Vi 

0 0 


The vector space se(3) is isomorphic to R 6 via the mapping £ 1 —> £ = 
( 11 , 01 ) € R 6 . 
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2.3 The exponential map 

For every £ £ T e G , let : R —> G denote the integral curve of the left 
invariant vector field X^ passing through e at t = 0. That is, = e 

and (t) = It follows from its definition that 


<M s + t) = 


which means that ^(t) is a one-parameter subgroup of G. Indeed, as 
functions of t, both sides are equal at t = 0 and both satisfy the differen¬ 
tial equation a(t ) = X^(a(t)) by left invariance of Xj, so they are equal 
by the uniqueness of integral curves. Using either left invariance or the 
fact that <j)£(t + s) = shows that 4>^(t) is defined for all t £ R. 

The function exp : T e G —> G defined by exp(£) = </>j( 1) is called the 
exponential map of the Lie algebra g into G. The exponential map takes 
the line £s £ g, s £ R, into a one-parameter subgroup of G, i.e., 


exp(£s) = <^(s). 


To verify this formula, note that for fixed s, the curve t i—> <p^(ts) which 
at t = 0 passes through e, satisfies the differential equation 


^M ts ) = sX dM ts )) = 


Since and 4>^(ts) satisfy the same differential equation and both 

pass through e at t = 0, it follows that cj) s ^(t) = Putting t = 1 

yields exp(£s) = 4>^(s). In fact, all connected one-parameter subgroups 
of G are obtained in this way. 

Differentiating the map exp(£s) = <f>^(s ) with respect to s at s = 0 
shows that T e exp = id^G- Therefore, by the inverse function theorem, 
exp : g —> G is a local diffeomorphism from a neighborhood of zero in g 
onto a neighborhood of e in G. If G is compact it can be shown that the 
exponential map is surjective. 

Example A.9. The exponential map on (R™, +) 

Consider R" with addition as the group operation. The Lie algebra of 
R” is R” with the trivial bracket and the integral curve of a left invariant 
vector field X v (x) = v is given by <fi v (t) = vt. Thus, (f> v {t) ox = x + vt 
and exp : R™ —> R” is the identity. 

Example A.10. The exponential map on GI(n,R) 

Let G = GL{n, R) c R nxn so that g = gl(n,R) = R raxn with [A,B\ = 
AB — BA. For every A £ gl(n, R), the mapping '■ R —> GL(n, R) 
given by 
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is a one-parameter subgroup because <1> A (0) = I and 

+n— 1 j^n 

(n - 1 )! 


d ^ . . 77 t n ~ 1 A r ' / \ a 

= E 5^nr = 

n—1 x 


which shows that $ A is an integral curve of the left invariant vector field 
Xa■ Therefore, the exponential map exp : g[(n,R) —> GL(n,M) is given 
by 

EE An 

exp(A) = $ A (1) = ^ . 

n —0 

Example A.11. The exponential map on SO( 3) 

Let G = SO(3). It was shown in Chapter 2 that exp 7 corresponds to a 
rotation about the vector u> £ R 3 by an angle ||w||. An explicit formula 
is given by Rodrigues’s formula: 


e u =1 


ra si " IM + H 


:(1- 


cos u; 


(A.3) 


Example A. 12. The exponential map on SE(3) 

For G = SE( 3), the Lie algebra can be identified with 4x4 matrices of 
the form 


£ = 


U! V 

0 0 


u>, v £ 


with [^ 1 ,^ 2 ] = ^ 1^2 — ^ 2 ^ 1 - The exponential map is given by 
expf = 
where 


7 v 


e s Av 

0 1 

, u> = 0 and exp £ = 

0 1 _ 




A = I + 


r (l-cos||w||) + 


M - sin M 


The exponential map exp : g — * G defined by exp(£) = <^(1), is a local 
diffeomorphism from a neighborhood of zero in g onto a neighborhood 
of e in G. Thus, restricted to a small neighborhood U of e there is a 
function log : U —> g such that exp o log(g) = g for all g £ U. The 
function log : U C G —> g is the inverse of exp : g —> G. For the general 
linear group, it can be computed explicitly. 

Example A.13. Log function on GL(n, R) 

Let G = GL(n,W) and A £ G. Then, the log function is defined by the 
following matrix polynominal 


logA = ]T(—1) 


n+1 {A-iy 


which converges for all || A — /|| < 1. 
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Example A.14. Log function on SO( 3) 

Let G = SO( 3). Then the log function is given by log R = a = OQ, where 
8 £ R and Q £ se(3) are given by 

2 cos 8 + 1 = trac e(R) and u) = —(R — R T ) R ^ I. 

2sm 8 

When R = I, 8 = 2nk for any integer k and u> can be chosen arbitrarily. 
Note that the log function on 50(3) is multi-valued since 8 is not unique. 

Example A. 15. Log function on SE( 3) 

The log function on SE( 3) is given by 


£ = 


log 


R 

P 


Q A 1 p 

0 

1 


0 0 


where u) = log R and 


A - 1 



2sin||u;|| - ||u;||(l + cos |M|)^ 2 

2|MI 2 sin !|w|| 


to 0. 


If u) = 0 then A = I. The log function on SE{ 3) is multi-valued since u> 
is not unique. 


2.4 Canonical coordinates on a Lie group 

Since exp : g —>■ G is a local diffeomorphism, we can use the exponential 
map to define local coordinates for G. Let { Xi ,..., X n } be a basis of 0 . 
The mapping 

g = exp(A'i<Ji H-h X n a n ) (A.4) 

defines a local diffeomorphism between the real numbers a £ R n and 
g £ G for g sufficiently near the identity. Hence we can consider a as 
a coordinate mapping, a : U —> R™, where U C G is an arbitrarily 
small neighborhood of the origin. Using this coordinate chart and left 
translation, we can construct an entire atlas for the Lie group G. Define 
a chart (Ug,i/) g ) about g £ G by letting 

U g =Lg(U) = {Lgh\h£U} 

and defining ip g = a o L g -i : U g —► R ra , so that 

'tpg(h) = <j(g~ l h). 

It is not difficult to show that the set of charts {{U g , ip g )} indeed forms 
an atlas for G. The functions (ay,..., a n ) defined in equation (A.4) 
are called canonical coordinates of the first kind around the identity and 
relative to the basis {X \,..., X n }. 
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If we write 


g = exp X\0\ exp X^Oi ■ ■ • exp X n 6 n (A.5) 

for g near the identity, then the functions (d\ 1 62 ,..., 0 n ) are called the 
canonical coordinates of the second kind. Examples of such coordinates 
are the product of exponentials formula studied in Chapter 3, and the 
Euler angle parameterizations of the rotation group. 

2.5 Actions of Lie groups 

In Chapter 2 we transformed points, vectors, twists, and wrenches using 
matrix multiplication with either g or some form of Ad g . All of these 
transformations can be described as the left action of SE( 3) on an appro¬ 
priate space. In this section, we give the definition of a left action of a Lie 
group on a manifold and give several examples related to robot kinemat¬ 
ics. More specific examples which make connections with the material in 
Chapter 2 are presented in the next section. 

Let M be a smooth manifold and G a Lie group. A left action of G 
on M is a smooth map such that 

(i) d>(e, x ) = x for all x £ M 

(ii) For every g,h £ G and x £ M, $(< 7 , 4?(h, x)) = $( 5 / 1 , x) 

We will often write $£,( 2 ) for $(< 7 , x). If $ is an action of G on M and 
x £ M, the orbit of x is defined by 

Orb x = {$ 9 (x) : g £ G}. 

Example A. 16. Action of GL(n,R) on R™ 

GL(n,M) acts on R” by (A,x) 1 —> Ax. The orbit of x ^ 0 is the open set 
R"/{0}. 

Example A.17. Action of G on itself via conjugation 

The map I g : G — * G given by I g (h) = R g -iL g (h) = ghg~ 1 is called the 
conjugation map or the inner automorphism associated with g. The map 
I g defines a left action on G since I e = id and 

I g o I h (x) = ghxh~ 1 g ~ 1 = I g h(x). 

Orbits of this action are called similarity classes. 

Example A. 18. Adjoint action of G on its Lie algebra 

Differentiating the conjugation map I g at e, we get the adjoint action of 
G on 0 , Ad :Gxg-tg defined as 

Ad s (£) = (T e I g )£ = T^Rg 1 o L g )£. 

If G is a subgroup of GL(n,M) then g C R" xn and Ad g £ = for 

£ e g. 
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Example A.19. Coadjoint action of G on the dual of its Lie 
algebra 

The coadjoint action of G on g*, the dual of the Lie algebra g of G, is 
defined as follows. Let Ad* : g* — > g* be the dual of Ad g defined by 

<Ad;a,0 = (a,Ad ff 0 

for a £ g* and £ £ g. Then the map <f>* : G x g* — > g* given by 

= Ad*_i a 

is the coadjoint action of G on g*. 

Example A.20. Lifted action of G from M to TM 

Let $ : G x M —> M be an action of G on M, where <f> 3 : M —> M is 
defined by = <L>(< 7 , x). One can lift this action to an action on TM, 

<f>* : G x TM —y TM, defined by 

$*( 5 , (x,v x )) = ($ g (x),T x $ g ■ v x ). 

<h* is called the lifted action of G on TM. 


3 The Geometry of the Euclidean Group 

In this section we study the geometric properties of the Euclidean group 
and discuss their implications on robot kinematics and control. The 
material in this section is based in part on the dissertation of Loncaric [63] 
and also on the work of the authors [59, 77, 86 ]. 

3.1 Basic properties 

In Chapter 2 we presented the theory of rigid body motion and showed 
its connections with homogeneous matrices and the theory of screws. We 
now show that the various tools available in the study of rigid motion are 
special cases of the more general tools defined for general Lie groups. 

Rigid body kinematics 

The configuration of a rigid body with respect to some reference config¬ 
uration is described by an element g = ( p,R ) £ SE( 3). If A is a fixed 
coordinate frame and B a frame attached to the rigid body, then we 
write g a b = ( p a b, Rab ) £ SE(3i) to denote the configuration of B with 
respect to A. p a b represents the location of the origin of the B frame and 
R a b £ SO( 3) its orientation. The group operation on SE( 3) allows us to 
determine the configuration of a frame C relative to A via an intermediate 
frame B: 

Sac — gab ' gbc — (Pab T RabPbci RabRbc) • 
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If we represent g £ SE( 3) as a 4 x 4 homogeneous matrix, 


then the group operation is given by matrix multiplication and we may 
regard SE( 3) as a subgroup of the general linear group, GL(4,R). 

The configuration g a & £ SE( 3) can also be interpreted as a mapping 
from the coordinates of a point written relative to the B frame into the 
coordinates of the same point written relative to the A frame. Formally, 
this defines an action of SE( 3) on R 3 given by <l> g (g) = p + Rq. In 
homogeneous coordinates this action can be written as 


Qa 


Rab 

Pab 


Qb 

1 


0 

1 


1 


It follows from associativity of matrix multiplication that this actually 
defines an action of SE(3) on R 3 . The use of a 1 in the last row of the 
homogeneous representation for a point allows the action of SE(3 ) on 
points to be represented as multiplication between a matrix and a vector. 

The action of SE( 3) on vectors describes how the velocity of a point 
is mapped from one coordinate frame to another. Formally, we represent 
the velocity of a point as an element of T x R 3 and the action of SE( 3) on 
tangent vectors (velocities) is the lifted action of SE(3) on M = R 3 . The 
lifted action of SE( 3) on TR 3 is given by 3> g *(v g ) = (g(q),Rv q ) where 
g(q) denotes the action of g on the point q. In homogeneous coordinates, 
the tangent space (velocity) portion of the action can be written as 


V a 


H-ab 

Pab 


Vb 

0 


0 

1 


0 


By defining the homogeneous representation of a vector to have a zero in 
the bottom row, we are able to once again use multiplication of a matrix 
and a vector to represent the action. 

Since SE( 3) is a Lie group, the exponential map can be used to map 
elements of the Lie algebra into the group. In homogeneous coordinates, 
the Lie algebra of SE( 3) is a Lie subalgebra of gl(4, R) consisting of ma¬ 
trices of the form 


f= 


U! V 

0 0 


(25 £ so( 3), oeR 3 , 


with the Lie bracket given by the matrix commutator. We call an element 
of the Lie algebra se(3) a twist. The vector space se(3) has dimension 6 
and is isomorphic to R 6 via the mapping £ h-> £ = (w, v). 

A twist can be interpreted geometrically using the theory of screws. 
Consider the motion generated by simultaneously rotating and translat¬ 
ing about an axis in the direction u> £ R 3 going through a point q £ R 3 . 
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Let h represent the ratio of translational motion to rotational motion. If 
h is finite, then the resulting rigid motion is the exponential of the twist 
£ £ se(3) given by 

Q —q x u) + hui 
0 0 

The one-parameter subgroup cj>^(0) = exp (£0) generated by this twist 
corresponds to a rotation about an axis followed by translation along that 
same axis. Thus the exponential of a twist generates a screw motion. 

It follows from the general properties of the exponential map that, 
near the identity, any element of SE( 3) can be written as the exponential 
of some twist. For SE( 3) it can be shown that the exponential map is 
actually surjective and hence any rigid transformation can be written 
as the exponential of some twist. This statement may be regarded as 
a restatement of Chasles’ theorem, which states that every rigid motion 
can be realized as a screw motion. 

Although every rigid motion can be written as the exponential of a 
twist, the set of twists do not define a parameterization for SE(3). The 
exponential map is not injective, and hence there may be many twists 
which give the same rigid motion. One example of this is a pure rotation, 
which can be written as either a rotation about an axis ui by an angle 9 
or a rotation about an axis — u> by an angle 2n — 9. These two different 
twists give the same motion. 

Velocities and forces 

If c(t) £ M is a curve on a manifold, then the velocity of that curve is 
an element of the tangent space to M at c(t), i.e., c(t) £ T c ( tj M. If M 
happens to be a Lie group, then the tangent space T g G is isomorphic 
to T e G. Hence, by left translation, we can identify the instantaneous 
velocity of a trajectory on a Lie group with an unique element of the 
corresponding Lie algebra. 

Returning to SE( 3), there are two ways to map T g SE{ 3) to T e SE(3 )— 
left and right translation. Consider left translation first. We make use 
of the fact that since SE{ 3) can be viewed as a matrix Lie group (using 
homogeneous coordinates), so the tangent map to Lh : SE( 3) —> SE( 3) is 
given by matrix multiplication: 



TgL h X g — hX g , 

where X g £ T g SE( 3). To show this relationship, let g(t) £ SE( 3) be a 
curve which is tangent to X g at time t = 0. Then from the definition of 
the tangent map, 

T g L h g{ 0) = j f {L h o g(t))| t=0 = hg( 0). 
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9(0) 


Figure A.l: Trajectory of a rigid body relative to a fixed frame. 

Letting h = g~ x we see that, using left translation, the velocity of the 
rigid body can be shifted to the identity and written as a twist 



This is precisely the body velocity that was defined in Chapter 2. 

If we use right translation to map a velocity g £ T g SE{ 3) to the 
tangent space at the identity, the resulting quantity is the spatial velocity: 



The derivation of this formula follows exactly as in the body velocity 
case, replacing Lh with Rh- 

One reason for the terminology “body” velocity and “spatial” velocity 
becomes clear if we consider the action of twists on points. Let q £ ffi 3 
be a point attached to a rigid body and let g a b{t) £ SE(3) describe the 
trajectory of a frame B attached to the rigid body relative to a fixed 
reference frame A , as shown in Figure A.l. In homogeneous coordinates, 
the trajectory of the point q as a function of time can be written as 


q a (t) = g a b(t)q b , 


where q a and qb are the coordinates of the point relative to the A and B 
frames. The velocity of the point, relative to the A frame, is given by 


v a (t) = q a {t) = g a b(t)q b , 


(A.6) 


where we have used the fact that q b is constant since the point is fixed 
in the body frame. Thus, g ab (t) £ T g SE( 3) can be viewed as a mapping 
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between the body coordinates of a point and the spatial velocity of that 
same point. 

A more appealing representation of velocity is one which does not 
require switching between coordinate frames. That is, suppose we wish 
to find the relationship between the coordinates of a point and its velocity, 
when both quantities are specified with respect to a single frame. We can 
accomplish this by transforming either the coordinates of the point or the 
coordinates of velocity to the appropriate frame. For example, if we are 
given the coordinates of the point q with respect to the spatial frame A , 
then the velocity of q with respect to A is given by 

v a = gqb = ( gg~ 1 )q a ■ 

This is precisely the spatial velocity that is defined by using right transla¬ 
tion to pull back the velocity g £ T g SE(3) to T e SE{ 3). A similar argument 
shows that the body velocity, g~ 1 g can be viewed as a map from the body 
coordinates of a point to the body velocity of that point. 

The body and spatial velocities are physically interpreted as the in¬ 
stantaneous translational and rotational velocity written relative to the 
body or spatial frame, respectively. They are related to one another by 
the adjoint action of SE( 3) on se(3). Namely, if V b is the body velocity 
for a rigid motion git), then the spatial velocity is given by 

V s = Ad s V b . 

This relationship can be derived by direct calculation, as in Chapter 2. 

A generalized force on SE( 3) can be regarded as a covector on SE( 3); 
i.e., an element of T*SE{ 3). As with velocities, we can map the cotan¬ 
gent space T*SE( 3) onto the dual of the Lie algebra by either left or 
right translation. We call the resulting object a wrench. Left translation 
corresponds to representing the force in the body coordinate frame while 
right translation corresponds to representing it in the spatial coordinate 
frame. The natural action of a wrench on a twist gives the instantaneous 
work due to applying the given wrench along the infinitesimal motion 
generated by the twist. 

If we identify se(3)* with R 6 , then a wrench can be written as 



where / is the translation component of the force and r is the angular 
component. The natural action of a wrench on a twist becomes the 
inner product between F £ R 6 and F £ I 6 . It is important to keep in 
mind, however, that this action is the natural action of a covector on a 
vector, and is defined independently of any inner product structure on 
R 6 S* se(3). 
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Although wrenches are dual to twists, we choose to represent them 
slightly differently. We always represent a wrench on SE( 3) as a vector 
in R 6 since the matrix representation of a wrench does not prove to 
be particularly useful. Furthermore, we write a wrench using a single 
subscript to denote the frame with respect to which it is written. Thus, 
if A is an inertial reference frame and B is a frame attached to a rigid 
body, we write F a for the spatial representation of a wrench applied to 
the rigid body and F b for the body representation of the wrench. In 
taking the action of a wrench on a twist, one must always insure that the 
twist and the wrench are represented relative to the same frame. Thus 
the instantaneous work generated by a twist V and a wrench F can be 
written either as V* b ■ F„ or V£ b ■ F, b . 


Transformation laws 


Let V bc E R 6 be the spatial velocity of a frame C relative to an inertial 
frame B and suppose that we wish to know the spatial velocity with 
respect to a different inertial frame, A. Using the definition of the spatial 
velocity, we have 

9ac 9ac = 9ab 9bc 9bc 9 a b 

gab V bc g ab = Ad Sat V bc , 



and hence the velocity of the rigid body is transformed according to the 
adjoint action of SE(3) on se(3). If we represent the velocities as vectors 
in R 6 then we write U a s c = Ad 9a6 V bc where Ad ff : R 6 —> R 6 is the matrix 


Ad g 


R pR 
0 R 


Note that here we use the symbol Ad ff to represent the adjoint mapping 
both on se(3) C R 4x4 and on R 6 , which is isomorphic to se(3). 

Suppose instead that we change the body coordinate frame. Let g ab 
represent the configuration of the frame B relative to A and let g bc repre¬ 
sent a fixed transformation corresponding to a new choice of body frame. 
The spatial velocity of C with respect to A is given by 


Vac — 9ac9 ac — 9a b 9bc9 bc 9 ab — V„ 


ab' 


Thus, changing the body coordinate frame does not affect the spatial 
velocity of the object. 

Similar relationships can be derived for body velocities. Changing 
the spatial coordinate frame does not affect the body velocity of a rigid 
object. However, if g ab represents the motion of the rigid body with 
respect to the frame A and g bc represents a new choice of body frame, 
then 


VL = Ad g -i Ul 
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Thus the adjoint action of SE( 3) on se(3) represents the effect of a change 
of body frame on the body velocity. 

The transformations described above assume that new and old body 
or spatial coordinate frames have a fixed relative configuration. If we 
are given two rigid motions g a b{t) and gbc(t), then in order to compute 
the velocity of frame C relative to frame A, we must add the velocities 
between the frames. This addition must occur in a single coordinate 
frame, and hence we use the adjoint mapping to transform the velocities 
appropriately. For example, the spatial velocity between frames A and C 
is given by 

V° c = V a % + Ad gab V b s c . 

The adjoint mapping in the second term converts the instantaneous ve¬ 
locity Vj®, which is written in the coordinates of frame B, into an instan¬ 
taneous velocity written relative to frame A. 

The coadjoint action is used to model the transformation of wrenches. 
If F a £ R 6 = se(3)* is a wrench written relative to a fixed frame A, then 
the coordinates of the wrench relative to a new fixed frame, B, are given 

by 

F b = Ad^-i F a 

y a b 

where g ab is the configuration of frame B relative to A. This expression 
follows directly from the definition of the coadjoint action given in the 
previous section. 


3.2 Metric properties of SE( 3) 

Since the Lie algebra of SE(3) can be identified with R 6 , there is an 
inner product structure on se(3) induced by the usual inner product on 
R 6 . However, it turns out that this inner product is not invariant under 
change of coordinate frame and hence can be misleading. Suppose, for 
example, that we are given two twists £i € R 6 and £2 £ R 6 that satisfy 
£1 ■ £2 = 0. If we transform the coordinate frame with respect to which 
the twists are written, then the twists transform as £' = Ad ff £,, where 
g £ SE( 3) represents the change of frame. The inner product between 
the twists in the new coordinate frame is given by 


£j • £2 = £1 


r t o' 

R pR 

£2 = £1 

I R T pR 

-R T P R t 

0 R 

-R T pR I-R T p 2 R 


&■ 


If p ^ 0, then the dot product is not preserved and hence two twists 
which are orthogonal relative to one choice of coordinate frame may not 
be orthogonal relative to a different choice of frame. 

This lack of frame independence has caused some confusion in the 
robotics literature, due to incorrect use of the inner product on R 6 as an 
inner product in se(3). In this section we show that there is no inner 
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product structure on se(3) which is invariant under change of coordinate 
frame. The implications of this fact are also discussed. 


Invariant metrics on R n 

Consider first the Euclidean space I". A metric \F on R” is a bilinear 
function $ : R” x I" ^ 1 such that 

1 . tj/ is symmetric: x I/(v,w) = \F(w;,v), for all v,w £ R". 

2. \F is positive-definite: ^(v,v) > 0 and \I/(v, v) = 0 if and only if 
v = 0. 

Let {ei,... ,e n } be an orthonormal basis of R™ (note that such a basis 
defines a Cartesian coordinate frame in R”). With respect to this basis 
’F has the matrix representation 

Ty- = T (e, ,ej), i,j = 1 ,... ,n. 

From now on, we will use *F to represent both the mapping and its matrix 
representation. Thus \1/(v, trx) = t/ T \I iw. 

Because a metric is positive definite, the matrix representation of a 
metric is always invertible. We will occasionally be interested in sym¬ 
metric bilinear mappings which have invertible matrix representations, 
but are not positive definite. A bilinear function *F : R" x R n —> R is 
nondegenerate if 'F(d,ui) = 0 for all w £ R ra implies that v = 0. We 
call a bilinear mapping which is symmetric and nondegenerate, but not 
necessarily positive-definite, a pseudo-metric. 

Let v a be the representation of a vector in frame A and Vb be the 
representation of the same vector in frame B. These vectors are related 
by the lifted action of SE(n ) on R" and hence 

Va — B a bVbj (A.7) 

where g a b = ( p a b, R a b ) £ SE(n) is the position and orientation of B 
relative to A. A metric is said to be invariant under change of coordinate 
frames if 


^(Rvi, Rv 2 ) = ^(rh, ^ 2 ) Vui, u 2 € R n , R £ SO(n). (A. 8 ) 


Equation (A. 8 ) yields the following constraint on the matrix representa¬ 
tion of T: 

mi = m \/R £ SO(n). (A.9) 

Lemma A.2. Let ’F £ R” xra be a symmetric matrix which satisfies equa¬ 
tion (A.9) for all R £ SO{n ) (i.e., tF commutes with all n x n rotation 
matrices). Then 

>F = al 


for some a £ R. 
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Proof. Let v be a unit eigenvector for \1/ with eigenvalue A. Multiplying 
both sides of equation (A.9) by v, we have 

^Rv = Rtyv = A Rv. 

It follows that Rv is an eigenvector of 'L for any R £ SO{n ) and hence 
'Stw = Xw for any unit vector w £ R n . This is only possible if is a 
scalar times the identity matrix. □ 

The scalar a corresponds to a choice of length scale on R n . In the case 
of R 3 , a length scale assigns a physical unit such as “meters” or “feet” to 
a unit vector in R 3 . Once a fixed length scale is chosen, there is a unique 
metric on R 3 which is invariant under change of coordinate frames. 

Proposition A.3. Invariant metrics on R" 

Up to a choice of length scale, the Euclidean space R n has a unique posi¬ 
tive definite metric which is invariant under change of coordinate frames. 

Invariant metrics on SE(3) 

A natural question that follows from the preceding discussion is whether 
SE( 3) also supports a metric that is invariant under change of coordinate 
frames. If so, we can associate a length measure to twists, generalizing the 
notion of speed in R 3 to se(3). Since SE( 3) is not a Euclidean space, but 
a manifold, we need to define what we mean by a metric on a manifold. 

Let M be a manifold. A quadratic form on M is a bilinear mapping 
'La, : T X M x T X M —> R which is symmetric for all x and varies smoothly 
with x. If SP is positive definite then we call 'L a metric. If T is non¬ 
degenerate but not positive-definite, we say that 'L is a pseudo-metric. 
Let F : M —> M be a smooth map. The pull back of a quadratic form T 
by F is also a quadratic form, denoted F*'L defined by 

F fL x{p x .,vjx) = (T x F v x , T X F w x ), v x ,m x £ T X M. 

'L is said to be F-invariant if F*'L = 'L. 

To define a metric on a Lie group, we may first define it at the identity 
and then translate it to the whole group by either left or right translation. 
The resulting metric is automatically left or right invariant, respectively. 
However, a left invariant metric may not be right invariant and vice versa. 
A metric that is both left and right invariant is called bi-invariant. Let 
Ih ■ SE( 3) —> SE( 3) be the conjugation map 

4 (5) = hgh~ 1 . 

Since Ih = Rh-i 0 Eh, a metric 'L is bi-invariant if and only if it is Ih- 
invariant for all h £ SE{3). That is, we require that a bi-invariant metric 
satisfy 

I*'L = 'L, Mh£SE( 3). 
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These same definitions hold for any quadratic form on SE( 3), even if it 
is not positive definite. 

Lemma A.4. Let T be a left (or right) invariant quadratic form on 
SE( 3). Then 'h is Ih-invariant if and only if it is Ih-invariant at the 
identity, i.e., 

d’ e {f,r]) = ^ , e (Ad ?l f,Ad? l 77) 
for all h £ SE(3) and £,,r} £ se( 3). 

Proof. The proof of necessity is immediate. To prove sufficiency, we only 
need to show that is right invariant since Ih = Rh- 1 ° Lh . For this, 
observe the following equations, which hold for all f,fj £ se(3) and h £ 
SE(3): 


&e(t,W) = ^e(T e Ihi, TJtjj) 

= T e(T e (L h o R h -i)£, T e (L h o R h -i)f ]) 

= d> e (T h -14 ° T e R h -il ;, T h -iL h o T e R h -ifj) 
= '^h- 1 {T e Rh- 1 f, T e R h -ifj). 


Here, the first equation follows from T being /^-invariant at the identity, 
and the last equation follows from left invariance of U/. This shows that 
'h is right invariant and hence bi-invariant. Q 

Proposition A.5. Bi-invariant quadratic forms on SE(3) 

Let T be a left invariant quadratic form on SE( 3), where we identify se(3) 
with R 6 . Then T is bi-invariant if and only if the matrix representation 
of if? at e has the form 


0 p I 


PI 7 1 ’ 


/3,7 £ R. 


Proof. By Lemma A.4, is bi-invariant if and only if for all £,fj £ se(3) 
and h £ SE( 3), 

*e(T e I h Z T e I h fj ) = ?})■ (A. 10) 

Identifying se(3) with R 6 and representing if? e by the matrix 


where A,B,C £ R 3x3 and A, C are symmetric, we have from equa¬ 
tion (A. 10) that 

Adh, Wi e SE( 3). (A.ll) 


A B 
B t C 


= ^d T h 
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Using the definition of the adjoint mapping on M 6 


Ad,, = 


R 

0 


pR 
R ’ 


equation (A.11) becomes 

A = R t AR 
B = R T ApR + R t BR 

C = -R T pApR - R T pBR + R T B T pR + R T CR. 


By Lemma A.2, A in equation (A.12) must be of the form 

A = al, atl. 


(A.12) 


Since (A.12) holds for all h € SE( 3), by letting p = 0 in the second and 
third equations we obtain 


B = pi C = ^I (3, yeR. 

Finally, using B = pi and C = 7 / in the third equation of (A.12), we 
conclude that a = 0 and the theorem is proven. Q 

Among the set of bi-invariant quadratic forms, we have the following 
two special cases. 

Example A.21. Hyperbolic metric on SE(3): (/3 = 1, 7 = 0) 

The quadratic form whose matrix representation given by 

)h = {vfoj 2 + ufuq) (A.13) 

is called the hyperbolic metric on SE( 3). The eigenvalues of the matrix 
representation of (•, •)# are all ±1 and occur pairwise. Hence it is possible 
for a vector to have negative “length” with respect to the hyperbolic 
metric. 

The choice of a hyperbolic metric on SE( 3) depends on a choice of 
length scale on R 6 . This is clear from the formula in equation (A. 13), 
where a change of length scale results in a linear factor in the hyperbolic 
metric. Thus, the choice of a particular hyperbolic metric fixes the ratio 
between a unit translation vector and a unit rotation vector. 

Although it is not positive definite, the hyperbolic metric is non¬ 
degenerate and hence it defines a pseudo-metric on SE( 3). In fact, <v>* 
coincides with the reciprocal product defined in Chapter 2. The hyper¬ 
bolic metric is a special case of the Klein form, which can be defined on 
any Lie group [13]. 


Vi 


V2 

Wi 


0J2 
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Example A.22. Killing form on SE( 3): (/3 = 0, 7 = —4) 

The quadratic form given by 


Vl 


V2 

fill 

5 

0J2 


)K 


—4uJ^UJ2 


(A.14) 


is called the Killing form on SE( 3). It is degenerate, and hence defines 
neither a metric nor a pseudo-metric. The Killing form on SE( 3) does 
not depend on the choice of length scale. 

The Killing form on SE( 3) is a special case of the Killing form for a 
general Lie group, defined as 


(£i,6)if = trace(ad£ -adg), 

where ad^-: g —> g : rj 1 —> [£, fj\. The factor of —4 in equation (A.14) was 
chosen to make it agree with the general definition. 

Observe that in neither of the above cases is the quadratic form pos¬ 
itive definite. In fact, it is easy to see that for any bi-invariant metric 
\H((u, 0), (v, 0)) = 0. Hence we have the following: 

Corollary A.5.1. Lack of a bi-invariant metric on SE( 3) 

There does not exist a bi-invariant (positive-definite) metric on SE(3). 

The fact that a bi-invariant metric does not exist on SE( 3) does not 
mean that one cannot define a notion of length on SE( 3). Rather, it 
implies that the definition of a metric is not intrinsic. It involves a choice. 
If we restrict ourselves to metrics which are defined at the identity and 
extended to the entire group via left (or right) translation, we have the 
following characterization: 

Proposition A. 6 . Left invariant metrics on SE( 3) 

The set of all left- (or right-) invariant metrics on SE{3) are parame¬ 
terized by the choice of reference frame origin and the choice of length 
scale. 


A proof of this proposition can be found in [63]. The need for a choice 
of reference frame origin follows from the lack of a bi-invariant metric on 
SE( 3). The choice of length scale affects how translational motion and 
rotational motion are weighted in the metric. 

We discuss some implications of the preceding results on robot kine¬ 
matics and control in the following examples. 

Example A.23. Geometric attributes of twists 

Let £ = (v,u}) be a twist with u> ^ 0 (the exponential of such a twist is 
referred to as a proper screw motion). The magnitude and pitch of 
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the twist, as defined in Chapter 2, can be re-interpreted using the Killing 
form and the hyperbolic metric as follows: 


Mz = -\(Z,Ok and ^ = -2^1^. 

4 \S,Qk 

Thus, both quantities are coordinate-frame independent attributes of a 
twist with nonzero angular component. 

The pitch of a screw obviously depends on the choice of length scale 
since it is defined as the ratio of translational and rotational motion. This 
extra freedom is reflected in the choice of length scale which is required 
for the hyperbolic metric. 

Example A.24. Constraints on SE( 3) and hybrid force control 

Consider the hybrid control problem, in which a manipulator pushes 
against a surface in R 3 . This surface can be described as a set of holo- 
nomic constraints of the form 


fi(g) = 0 i = l,...,k, 

where each /) : SE( 3) —> R is a smooth function. Differentiating this 
position constraint with respect to time, we get a constraint on g £ 
TgSE{ 3): 

dfi(g)g = ( dfi(g),g) = 0, (A.15) 

where dfi(g) £ T*SE( 3) and g € T g SE( 3). This constraint can be trans¬ 
lated back to the identity using left (or right) translation, 

(dfi(g),g) = {{dfi(g),(L g )*(L g -i)*g) 

= {L* g df(g),(L g -i)*g} 

= (L* g dfi{gU) = 0, 


where £ = g~ 1 g £ se( 3) and L*dfi(g) £ se(3)*. 

The quantity L* g dfi{g) is an element of se(3)*, the dual of the Lie 
algebra, and hence we can interpret it as a force. The subspace of se(3)* 
spanned by L*dfi, ..., L*dfk is called the space of constraint forces, and 
the subspace of se(3) annihilated by L*dfi,i = 1 ,...,m, is called the 
space of free motions. These subspaces annihilate each other since any 
constraint force acting on a free motion gives zero instantaneous work. 
Although the subspaces themselves are not invariant under change of 
coordinates, it is always true that these subspaces will annihilate each 
other, independent of the choice of frames used to represent the problem. 

A common mistake in the robotics literature is to treat the constraint 
directions as twists. In this case, one tries to define the action of a 
constraint force on a velocity as the usual dot product on R 6 . Constraint 
forces are then identified with those twists that are orthogonal to the 
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space of free motions. However, this description is not frame invariant. 
If we change the frames which are used to describe the problem, the space 
of constraint twists may no longer be orthogonal to the free motions. This 
is because there is no invariant metric on SE( 3) and hence there is no 
invariant notion of orthogonality. 

This apparent problem is completely due to associating the constraint 
directions with vectors instead of covectors. The proper way to describe 
a constraint is as an element of the dual of the Lie algebra. In this case, 
if we shift coordinate frames, then a twist b 6 l 6 = se(3) and a wrench 
F G R 6 = se(3)* will transform according to 

V' = Ad ff V and F' = AdJ-i F. 

The action of the new twist on the new wrench is given by 

F'-V' = ( F') t Ad g -i Ad g V = F-V. 

Note that the action of a wrench on a twist also scales correctly with 
respect to a change of length scale. So a wrench which annihilates a twist 
does so independently of the units in which length is described. 

Example A.25. Left invariant constraints 

The constraints described in the previous example generate a subspace 
of twists and wrenches which annihilate each other. These subspaces 
are defined on the Lie algebra and its dual, but they may vary as the 
manipulator moves along the constraint surface. That is, the constraint 
df(g) G T*SE(3) pulled back to the identity, L*df(g) € TfSE{ 3), may 
depend on <7 G SE{ 3). 

A constraint is said to be left invariant if L*df(g) = df(e). In other 
words, a left invariant constraint gives directions of free motion which 
are constant relative to a body fixed frame. Left invariant constraints 
arise frequently in applications, for example when moving across a flat 
surface. The following result, due to Loncaric [63] asserts that all left (or 
right) invariant constraints can be characterized in terms of subgroups of 
SE( 3). 

Proposition A.7 (Loncaric). A constraint f(g) = 0 is left (or right) 
invariant if and only *// _1 (0) is a subgroup of SE(3). 

Example A.26. Manipulability measures 

In Chapter 3, we defined several manipulability measures based on prop¬ 
erties of the manipulator Jacobian. Some of these manipulability mea¬ 
sures rely on the Euclidean metric on R 6 = se(3), which we now know is 
not invariant under change of coordinate frames. Thus, care should be 
exercised when one applies these manipulability measures to manipulator 
design and control. 
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To illustrate our point, let us place two tool frames Tj and T 2 at 
the end-effector of a robot and then write down the (body) manipulator 
Jacobian at a joint position 9. Let J\{9) and J 2 (0) be the representa¬ 
tion of the body manipulator Jacobian with respect to frame Tj and T 2 , 
respectively. Then, we have 

J 1 (9)=Ad h J 2 (9), 

where h £ SE(3) is the position and orientation of T 2 relative to Tj. Since 
Adh does not preserve the Euclidean metric, we have in general 


± cr min (J 2 (6 1 )). 


Furthermore, the point in the joint space at which the minimum singular 
value of Ji is achieved could be different from that of J 2 . This shows that 
manipulator design based on maximizing the minimum singular value of 
J is ill-posed, as the result depends on placement of the body coordinate 
frames. 

A similar result holds for the condition number. 

3.3 Volume forms on SE(3) 

In the last section we saw that there does not exist a bi-invariant metric on 
SE( 3) and hence the definition of the length of a curve on SE( 3) depends 
on the choice of a reference frame and a length scale. One would like 
to know to what extent the choice of a reference frame and length scale 
affects the definition of volume on SE{ 3). This is important, for example, 
when talking about the size of the workspace of a robot manipulator, 
which is a compact subset of SE( 3). We begin with a very brief review 
of the definition of volume on a manifold. See Boothby [9] for a more 
complete treatment. 

Let M be a manifold of dimension n. A volume form ft on M is a 
skew-symmetric multilinear map 

ftp : T p M x • • • T p M -> R 

V_ v _✓ 

n 

such that ftp ^ 0 for all p £ M. The volume form defines the volume of 
a parallelepiped formed by n tangent vectors. By integrating the volume 
form over a manifold (using local coordinates), the volume of the manifold 
(or a subset of the manifold) can be defined. If M = R" then the standard 
volume form which corresponds to ordinary integration is given by the 
determinant function, as follows: 

ft(Vi,. .., V n ) = det \Vi ••• V n ] Vi £ R”. 
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Let F : M —> M be a smooth map from M to itself. The pull back of 
ft, by F, is a volume form F*Sl defined by 

F*Q x (v i, ...,v„) = Q. F ( x ){T x Fv i,.. .,T x Fv n ). 

In local coordinates, if J is the Jacobian of F evaluated at x and det J 
the determinant of J , then 


F*fl = {detJ)n. (A.16) 

Thus the pull back of a volume form generalizes the change of variables 
formula from ordinary calculus. A volume form on SE(3) is said to be 
left invariant if (L g )*Ll = fi and right invariant if ( R g )*Q = ft. A volume 
form which is both left and right invariant is called bi-invariant. 

Theorem A.8. Bi-invariant volume forms on SE( 3) 

Let Q be a left invariant volume form on SE{3). Then is also right 
invariant and hence bi-invariant. 


Sketch of proof. In order to show that it suffices to show that 

f1(e) = det (T e Ih)Ll(e). 

However, the Jacobian of /^ at the identity is just the adjoint mapping 
associated with h , and computing the determinant of Ad/j gives 


det Adh = det 


R pR 
0 R 


= 1 . 


Hence fl is bi-invariant. Right invariance follows from bi-invariance and 
left invariance. □ 

A corollary of this theorem is that SE( 3) supports a bi-invariant vol¬ 
ume form, since we can define the volume element on se(3) = R 6 and then 
use left (or right) translation to define a volume form on all of TSE{ 3). 
One possible choice for such a volume form is to multiply the standard 
volume form 3 on T e R 3 = R 3 with a volume form on so(3), the Lie 
algebra of 50(3). It can be shown that a volume form on 50(3) can be 
defined by making use of the Lie bracket and Killing form on 50(3): 

^SO( 3 )( w lj w 2 , W 3 ) = —-(wi, \u>2iW$)k = u>l ■ 0J2 x U>3- 

As seen from the last equality, this corresponds to the triple product if 
we identify so(3) with R 3 and, in fact, this is equivalent to the volume 
form defined by using the standard volume on R 3 . With this definition, 
50(3) has a volume of 87 t 3 rad 3 . 

A volume form defined by combining the volume on R 3 with that on 
50(3), i.e., LIse{ 3) = Or3 x Qso( 3), clearly depends on the choice of 
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length scale. In essence, this describes the tradeoff between translational 
volume and angular volume. However, once a length scale has been fixed, 
the volume form will not depend on the choice of coordinate frames. Also, 
since changing the length scale results in a corresponding scaling of vol¬ 
ume, one can still say that one volume is larger than another, independent 
of this choice. 

Example A.27. Well-posed manipulability measures 

The use of the determinant of J{0) as a measure of manipulability was de¬ 
scribed in Chapter 3. This manipulability measure makes use of a volume 
form on SE( 3) defined in terms of the standard volume on R 6 = se(3). 
Using the notation defined in Example A.26, the coordinate independence 
of this measure can be seen from the relationships 

det(Ji(0)) = det(Adh J2(0)) = det Ad^ det J2{0) = det J2(0). 

Example A.28. Optimal manipulator design [85, 86] 

Consider a robot manipulator consisting of six revolute joints (commonly 
called a 6R manipulator). Let 




g{6) = e' 


be the forward kinematics map of the manipulator relative to some refer¬ 
ence configuration, and l^ i be the twist axis associated with the ith joint. 
Let Cg denote the set of all curves which join the axes of the joints of 
the manipulator. Thus c £ Cf is a curve which links the axes of each 
and, without loss of generality, is parameterized such that c(i) C l^ ^ 0. 
In other words, c : [1,6] —> R 3 and c[i) is a point on the axis of the 
ith joint twist. The line segments c^j+i], i = 1,..., 5, are referred to as 
links of the manipulator, and c t denotes c evaluated at t. We define the 
manipulator length as 


6 



It is left as an exercise to show that Lm is independent of the choice of 
reference configurations. 

The work volume Vm of the manipulator is given by the following 
volume integral 



where Q = S 1 x • • • x S 1 = T 6 is the manipulator joint space, g is the 
forward kinematics map, and Q is a bi-invariant volume form on SE( 3). 
If we use the volume form CIse( 3 ) = Ur3 x flso( 3), the manipulator work 
volume in terms of the manipulator length, Lm, is 
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Figure A.2: An elbow manipulator 


A manipulator consisting of six revolute joints has maximal work volume 
if it achieves this bound. This bound is independent of choice of base and 
tool frames, but it does depend on the choice of length scale. However, 
as noted above, this choice of scale does not affect comparisons between 
volumes, and hence we can use this choice of volume to define an optimal 
manipulator. 

For the case of 6R manipulators, it is possible to completely describe 
the class of manipulators which achieve maximal work volume. Define an 
elbow manipulator to be one which consists of a shoulder, an elbow, and 
a wrist aligned as shown in Figure A.2. The wrist and shoulder joints of 
an elbow manipulator consist of mutually orthogonal axes intersecting at 
a point. The inverse to an elbow manipulator is one in which the wrist 
and the shoulder joints are switched (so that the “wrist” is located at the 
base of the manipulator). 

Theorem A.9. Optimal manipulator design 

A 6R manipulator with given length Lm has maximal work volume if and 
only if it is an elbow manipulator or the inverse of an elbow manipulator, 
with the elbow midway between the shoulder and the wrist. 

A detailed discussion and proof of this result is given by Paden and 
Sastry [85, 86]. The wrist at one end of the manipulator insures that 
all orientations can be reached at any configuration. The location of the 
elbow, at the midpoint between the shoulder and wrist, insures that there 
is not a hole in the center of the workspace. 
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Appendix B 

A Mathematica Package 
for Screw Calculus 


This appendix contains a brief description of a Mathematica package, 
Screws.m, which facilitates the use of screws, twists, and wrenches for 
analyzing robot kinematics. The Screws package implements all of the 
functions described in Chapter 2 and, when combined with the supple¬ 
mentary package RobotLinks .m, allows symbolic and numerical compu¬ 
tation of the kinematics of open-chain robot manipulators as well as many 
other functions. The Mathematica program itself is described in [121]. 

The Screws package is available via anonymous ftp from the host 
avalon.caltech.edu and may be used free of charge. Documentation 
and installation instructions are included with the source code for the 
package. The Screws package was written by R. Murray and S. Sur at 
the California Institute of Technology. All correspondence concerning the 
software should be sent to via e-mail to murray@avalon.caltech.edu. 
The authors assume no responsibility for the correctness or maintenance 
of the Screws package. The source code is currently available only via 
anonymous ftp. 

The remainder of this appendix contains a brief description of the 
Screws package, describing the functions which are available and their 
syntax. Although not strictly necessary, some familiarity with Mathe¬ 
matica is assumed. This appendix can also be used as a guide for im¬ 
plementing a screw calculus package in other symbolic and numerical 
programming languages. 


Using the Screws package 

The Screws package implements screw theory in 3-dimensional Euclidean 
space, R 3 . It uses homogeneous coordinates to represent points, vectors, 
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and rigid motions, making it easy to integrate into other Mathematica 
packages. 

The Screws package consists of two groups of functions. The first 
group operates on rotation matrices and implements all of the mathe¬ 
matical operations described in Section 2 of Chapter 2. The following 
functions are defined for computing in SO( 3): 

• AxisToSkew[w] 

Generate a skew-symmetric matrix give a vector w £ R 3 . 

• RotationAxis[R] 

Calculate the axis of rotation for a matrix R £ SO( 3). 

• SkewExp[S, theta] 

Calculate the exponential of a skew-symmetric matrix. If theta is 
not specified, it defaults to 1. If the first argument to SkewExp is 
a vector, SkewExp first converts it to a skew-symmetric matrix and 
then takes its exponential. 

• SkewToAxis[S] 

Generates a vector given a skew-symmetric matrix. 

Limited error checking is used to insure that the arguments to the func¬ 
tions are in the proper form. 

The second group of functions implements calculations on SE{ 3). 
Rigid body transformations are represented using 4x4 matrices. Func¬ 
tions are provided for transforming points and vectors to and from ho¬ 
mogeneous coordinates, as well as converting a translation and rotation 
pair into a 4 x 4 matrix. The following functions are defined for use in 
SE(3): 

• HomogeneousToTwist[xi] 

Convert xi from a 4 x 4 matrix to a 6-vector. 

• PointToHomogeneous[q] 

Generate the homogeneous representation of a point q € R 3 . 

• RigidAdjoint[g] 

Generate the adjoint matrix corresponding to g. 

• RigidOrientation[g] 

Extract the rotation matrix R from a homogeneous matrix g. 

• RigidPosition[g] 

Extract the position vector p from a homogeneous matrix g. 

• RigidTwist[g] 

Compute the twist xi £ R 6 which generates the homogeneous ma¬ 
trix g. 
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• RPToHomogeneous[R,p] 

Construct a 4 x 4 homogeneous matrix from a rotation matrix R 
and a translation p. 

• ScrewToTwist[h, q, w] 

Return the twist coordinates of a screw with pitch h through the 
point q and in the direction w. If h == Infinity, then a pure 
translational twist is generated. In this case, q is ignored and w 
gives the direction of translation. 

• TwistAxis[xi] 

Compute the axis of the screw corresponding to a twist. The axis 
is represented as a pair {q, w}, where q is a point on the axis and 
w is a unit vector describing the direction of the axis. The twist xi 
can be specified either as a 6-vector or a 4 x 4 matrix. 

• TwistExp[xi, theta] 

Compute the matrix exponential of a twist xi. The default value 
of theta is 1. If the first argument to TwistExp is a 6-vector, it is 
automatically converted to a 4 x 4 matrix. 

• TwistPitchfxi] 

Compute the pitch of a twist. 

• TwistMagnitude[xi] 

Compute the magnitude of a twist. 

• TwistToHomogeneous [xi] 

Convert xi from a 6-vector to a 4 x 4 matrix. 

• VectorToHomogeneous[q] 

Generate the homogeneous representation of a vector. 

Limited error checking is used to insure that the arguments to the func¬ 
tions are in the proper form. 

Manipulator kinematics 

The functions defined in the Screws package can be used to analyze the 
kinematics of a robot manipulator. This section describes this process and 
defines some new functions which streamline the analysis of manipulator 
kinematics. These functions are contained in the package RobotLinks .m, 
which is included with in Screws package distribution. 

The forward kinematics for a robot manipulator can be written as a 
product of exponentials (of twists). The following functions are defined 
for creating twists specifically for robot manipulators: 
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• RevoluteTwist[q, w] 

Construct the unit twist corresponding to a revolute joint in the 
direction w going through the point q. 

• PrismaticTwist[q, w] 

Construct the unit twist corresponding to a prismatic joint in the 
direction w going through the point q. 

These functions use the ScrewToTwist function defined in Screws.m. 

Once the twists are defined, the forward kinematic map and the ma¬ 
nipulator Jacobian can be calculated using matrix multiplication com¬ 
bined with the TwistExp and RigidAdjoint functions. These computa¬ 
tions are automated by the following functions: 

• ForwardKinematics [{xil, thl}, {xi2, th2}, ..., gstO] 

Compute the forward kinematics map using the product of expo¬ 
nentials formula. The pairs {xi, th} define the joint twist and 
joint angle (or displacement) for each joint of the manipulator. 

• SpatialJacobian[{xil, thl}, {xi2, th2}, ..., gstO] 

Compute the spatial manipulator Jacobian for the manipulator. 
The pairs {xi, th} are given as in the ForwardKinematics func¬ 
tion. 

An example of the usage of Screws and RobotLinks packages is shown be¬ 
low for computing the kinematics of a SCARA manipulator. The notation 
corresponds to the notation used to describe the SCARA manipulator in 
Chapter 2. 

<<Screws.m (* screws package *) 

<<RobotLinks.m (* additional functions *) 

(* Twist axes for SCARA robot, starting from the base *) 
xil = RevoluteTwist[{0,0,0}, {0,0,1}]; (* base *) 

xi2 = RevoluteTwist[{0,11,0}, {0,0,1}]; (* elbow *) 

xi3 = RevoluteTwist[{0,11+12,0}, {0,0,1}]; (* wrist *) 

xi4 = PrismaticTwist[{0,0,0}, {0,0,1}]; 

(* Location of the tool frame at reference configuration *) 
gstO = RPToHomogeneous[IdentityMatrix[3], {0,11+12,0}]; 

(* Forward kinematics map *) 
gst = Simplify! 

ForwardKinematics[ 

{xil,thl}, {xi2,th2}, {xi3,th3}, {xi4,th4}, gstO 

] 

]; 


438 



(* Spatial manipulator Jacobian *) 

Js = Simplify[ 

SpatialJacobian[{xil,thl}, {xi2,th2}, {xi3,th3}-, {xi4,th4}, gstO] 

]; 


439 


440 


Bibliography 


[1] J. Angeles. Rational Kinematics. Springer-Verlag, 1988. 

[2] H. Asada and J. J. Slotine. Robot Analysis and Control. John Wiley, 
1986. 

[3] K. J. Astrom and B. Wittenmark. Computer Controlled Systems: Theory 
and Design. Prentice-Hall, 1984. 

[4] J. Baillieul. Geometric methods for nonlinear optimal control problems. 
Journal of Optimization Theory and Applications, 25(4):519-548, 1978. 

[5] J. Baillieul and D. Martin. Resolution of redundancy. In R. W. Brock- 
ett, editor, Robotics: Proceedings of Symposia in Applied Mathematics, 
Volume 41, pages 49-90. American Mathematical Society, 1990. 

[6] R. S. Ball. A Treatise on the Theory of Screws. Cambridge University 
Press, 1900. 

[7] A. Bicchi, J. K. Salisbury, and D. L. Brock. Contact sensing from force 
measurements. International Journal of Robotics Research, 12(3):249- 
262, 1993. 

[8] A. M. Bloch, M. Reyhanoglu, and N. H. McClamroch. Control and 
stabilization of nonholonomic dynamic systems. IEEE Transactions on 
Automatic Control, 37(11):1746-1757, 1992. 

[9] W. M. Boothby. An Introduction to Differentiable Manifolds and Rie- 
mannian Geometry. Academic Press, second edition, 1986. 

[10] O. Bottema and B. Roth. Theoretical Kinematics. North-Holland, 1979. 

[11] R. W. Brockett. Control theory and singular Riemannian geometry. In 
P. Hinton and G. Young, editors, New Directions in Applied Mathematics, 
pages 11-27. Springer-Verlag, New York, 1981. 

[12] R. W. Brockett. Robotic manipulators and the product of exponentials 
formula. In P. A. Fuhrman, editor, Mathematical Theory of Networks 
and Systems, pages 120-129. Springer-Verlag, 1984. 

[13] R. W. Brockett, editor. Robotics: Proceedings of Symposia in Applied 
Mathematics, Volume 41. American Mathematical Society, 1990. 

[14] R. W. Brockett and L. Dai. Non-holonomic kinematics and the role of 
elliptic functions in constructive controllability. In Z. Li and J. F. Canny, 
editors, Nonholonomic Motion Planning, pages 1-22. Kluwer, 1993. 


441 


[15] R. W. Brockett, A. Stokes, and F. Park. A geometrical formulation of the 
dynamical equations describing kinematic chains. In IEEE International 
Conference on Robotics and Automation, pages 637-642, 1993. 

[16] R. A. Brooks and A. M. Flynn. Rover on a chip. Aerospace America, 
pages 22-26, October 1989. 

[17] P. Chiacchio and B. Siciliano. A closed-loop Jacobian transpose scheme 
for solving the inverse kinematics of nonredundant and redundant wrists. 
Journal of Robotics Systems, 6(5):601-630, 1989. 

[18] D. S. Childress. Artificial hand mechanisms. In Mechanisms Confer¬ 
ence and International Symposium on Gearing and Transmissions, San 
Francisco, CA, October 1972. 

[19] M. Cohn, D. C. Deno, S. S. Sastry, and J. Wendlandt. Actuating and 
force-sensing for cable-driven, teleoperated manipulators. In Medicine 
Meets Virtual Reality, San Diego, 1992. Aligned Management Associates. 

[20] P. Coiffet et al. Robot Technology. McGraw-Hill, 1983. Translation from 
the French, Les Robots (8 volumes). 

[21] J. J. Craig. Introduction to Robotics: Mechanics and Control. Addison- 
Wesley, second edition, 1989. 

[22] M. R. Cutkosky. Robotic Grasping and Fine Manipulation. Kluwer, 1985. 

[23] A. De Luca and G. Oriolo. The reduced gradient method for solving 
redundancy in robot arms. Robotersysteme, 7(2):117-122, 1991. 

[24] J. Demmel, G. Lafferriere, J. Schwartz, and M. Sharir. Theoretical and 
experimental studies using a multifinger planar manipulator. In IEEE 
International Conference on Robotics and Automation, pages 390-395, 
1988. 

[25] J. Denavit and R. S. Hartenberg. A kinematic notation for lower-pair 
mechanisms based on matrices. Journal of Applied Mechanics, pages 
215-221, June 1955. 

[26] D. C. Deno, R. M. Murray, K. S. J. Pister, and S. S. Sastry. Finger-like 
biomechanical robots. In IEEE International Conference on Robotics and 
Automation, 1992. 

[27] M. P. do Carmo. Differential Geometry of Curves and Surfaces. Prentice- 
Hall, 1976. 

[28] J. Duffy. Analysis of Mechanisms and Robot Manipulators. Edward 
Arnold Ltd., London, 1980. 

[29] J. Duffy and C. Crane. A displacement analysis of the general spatial 7R 
mechanism. Mechanisms and Machine Theory, 15:153-169, 1980. 

[30] A. G. Erdman and G. N. Sandor. Mechanism Design: Analysis and 
Synthesis. Prentice-Hall, 1984. 

[31] R. S. Fearing. Micro structures and micro actuators for implementing 
sub-millimeter robots. In H. S. Tzou and T. Fukuda, editors, Precision 
Sensor, Actuators and Systems. Kluwer Academic Publishers, 1992. 


442 


[32] C. Fernandes, L. Gurvits, and Z. Li. Attitude control of a space plat¬ 
form manipulator system using internal motion. International Journal of 
Robotics Research, 1994. (to appear). 

[33] C. Fernandes, L. Gurvits, and Z. Li. Near optimal nonholonomic motion 
planning for a system of coupled rigid bodies. IEEE Transactions on 
Automatic Control, March 1994. 

[34] G. F. Franklin, J. D. Powell, and A. Emami-Naeini. Feedback Control of 
Dynamic Systems. Addison-Wesley, second edition, 1991. 

[35] K. S. Fu, R. C. Gonzalez, and C. S. G. Lee. Robotics: Control, Sensing, 
Vision and Intelligence. McGraw Hill, 1987. 

[36] B. Gorla and M. Renaud. Modeles des Robots Manipulateurs: applications 
a leur commande. Cepadues-Editions, 1984. 

[37] M. Grayson and R. Grossman. Models for free nilpotent Lie algebras. 
Journal of Algebra, 135(1):177-191, 1990. 

[38] M. Hall. The Theory of Groups. Macmillan, 1959. 

[39] H. Hanafusa and H. Asada. A robotic hand with elastic fingers and its 
application to assembly process. In M. Brady et al., editor, Robot Motion: 
Planning and Control, pages 337-360. MIT Press, 1982. 

[40] R. Hermann and A. J. Krener. Nonlinear controllability and observability. 
IEEE Transactions on Automatic Control, AC-22:728-740, 1977. 

[41] G. Hinton. Some computational solutions to Bernstein’s problems. In 
H. T. A. Whiting, editor, Human Motor Actions—Bernstein Reassessed, 
chapter 4b. Elsevier Science Publishers B.V., 1984. 

[42] K. H. Hunt. Kinematic Geometry of Mechanisms. Oxford University 
Press, 1978. 

[43] A. Isidori. Nonlinear Control Systems. Springer-Verlag, 2nd edition, 
1989. 

[44] S. Jacobsen, J. Wood, K. Bigger, and E. Iverson. The Utah/MIT hand: 
Work in progress. International Journal of Robotics Research, 4(3) :21—50, 
1984. 

[45] A. Jain and G. Rodriguez. Recursive flexible multibody system dynamics 
using spatial operators. Journal of Guidance, Control, and Dynamics, 
15(6):1453-1466, 1992. 

[46] W. Kahan. Lectures on computational aspects of geometry. Department 
of Electrical Engineering and Computer Sciences, University of Califor¬ 
nia, Berkeley. Unpublished, July 1983. 

[47] I. Kao and M. R. Cutkosky. Quasi-static manipulation with compliance 
and sliding. International Journal of Robotics Research, ll(l):20-40, 
1992. 

[48] J. Kerr. An Analysis of Multi-fingered Hands. PhD thesis, Department 
of Mechanical Engineering, Stanford University, 1984. 

[49] H. K. Khalil. Nonlinear Systems. Macmillan, 1992. 


443 


[50] O. Khatib. A unified approach for motion and force control of robot ma¬ 
nipulators: The operational space formulation. IEEE Journal on Robotics 
and Automation, RA-3(l):43-53, February 1987. 

[51] D. Koditschek. Natural motion for robot arms. In IEEE Control and 
Decision Conference, pages 733-735, 1984. 

[52] A. J. Koivo. Fundamentals for Control of Robotic Manipulators. Wiley, 
1989. 

[53] K. Kreutz. On manipulator control by exact linearization. IEEE Trans¬ 
actions on Automatic Control, 34(7):763-767, 1989. 

[54] G. Lafferriere and H. J. Sussmann. A differential geometric approach 
to motion planning. In Z. Li and J. F. Canny, editors, Nonholonomic 
Motion Planning, pages 235-270. Kluwer, 1993. 

[55] J.-P. Laumond. Finding collision-free smooth trajectories for a non¬ 
holonomic mobile robot. In International Joint Conference on Artificial 
Intelligence, pages 1120-1123, 1987. 

[56] D. F. Lawden. Elliptic Functions and Applications. Springer-Verlag, 
1980. 

[57] H. Y. Lee and C. G. Liang. A new vector theory for the analysis of spatial 
mechanisms. Mechanisms and Machine Theory, 23(3):209-217, 1988. 

[58] Z. Li. Kinematics, Planning and Control of Dextrous Robot Hands. PhD 
thesis, Department of Electrical Engineering and Computer Sciences, 
University of California, Berkeley, 1989. 

[59] Z. Li. Geometrical considerations of robot kinematics. International 
Journal of Robotics and Automation, 5(3):139-145, 1990. 

[60] Z. Li and J. Canny. Motion of two rigid bodies with rolling constraint. 
IEEE Transactions on Robotics and Automation, 6(1):62-71, 1990. 

[61] Z. Li and J. F. Canny, editors. Nonholonomic Motion Planning. Kluwer, 
1992. 

[62] Z. Li, P. Hsu, and S. S. Sastry. Grasping and coordinated manipulation by 
a multifingered robot hand. International Journal of Robotics Research, 
8(4):33-50, 1989. 

[63] J. Loncaric. Geometric Analysis of Compliant Mechanisms in Robotics. 
PhD thesis, Division of Applied Sciences, Harvard University, 1985. 

[64] D. Manocha and J. F. Canny. Real time inverse kinematics for general 
6R manipulators. Technical Report ESRC 92-2, University of California, 
Berkeley, 1992. 

[65] R. Manseur and K. L. Doty. A robot manipulator with 16 real inverse 
kinematic solutions. International Journal of Robotics Research, 8654:75— 
79, 1989. 

[66] X. Markenscoff, L. Ni, and C. H. Papadimitriou. The geometry of grasp¬ 
ing. International Journal of Robotics Research, 9( 1):61—74, 1990. 

[67] J. E. Marsden. Lectures on Mechanics. London Mathematical Society, 
1992. 


444 


[68] J. E. Marsden, R. Montgomery, and T. S. Ratiu. Reduction, Symmetry, 
and Phases in Mechanics, volume 436 of Memoirs. American Mathemat¬ 
ical Society, 1990. 

[69] M. T. Mason and J. K. Salisbury. Robot Hands and the Mechanics of 
Manipulation. MIT Press, 1985. 

[70] J. M. McCarthy. An Introduction to Theoretical Kinematics. MIT Press, 
1990. 

[71] B. Mishra, J. T. Schwartz, and M. Sharir. On the existence and synthesis 
of multifingered positive grips. Algorithmica, 2:541-558, 1987. 

[72] D. J. Montana. The kinematics of contact and grasp. International 
Journal of Robotics Research, 7(3):17-32, 1988. 

[73] D. J. Montana. The kinematics of contact with compliance. In IEEE 
International Conference on Robotics and Automation, pages 770-775, 
1989. 

[74] R. M. Murray. Nilpotent bases for a class of non-integrable distribu¬ 
tions with applications to trajectory generation for nonholonomic sys¬ 
tems. Technical Report CIT/CDS 92-002, California Institute of Tech¬ 
nology, October 1992. 

[75] R. M. Murray, D. C. Deno, K. S. J. Pister, and S. S. Sastry. Control 
primitives for robot systems. IEEE Transactions on Systems, Man and 
Cybernetics, 22(1):183-193, 1992. 

[76] R. M. Murray and S. S. Sastry. Control experiments in planar manipu¬ 
lation and grasping. In IEEE International Conference on Robotics and 
Automation, pages 624-629, 1989. 

[77] R. M. Murray and S. S. Sastry. Grasping and manipulation using multi- 
fingered robot hands. In R. W. Brockett, editor, Robotics: Proceedings of 
Symposia in Applied Mathematics, Volume 41, pages 91 128. American 
Mathematical Society, 1990. 

[78] R. M. Murray and S. S. Sastry. Nonholonomic motion planning: Steering 
using sinusoids. IEEE Transactions on Automatic Control, 38(5):700- 
716, 1993. 

[79] Y. Nakamura. Advanced Robotics: Redundancy and Optimization. 
Addison-Wesley, 1991. 

[80] Y. Nakamura, K. Nagai, and T. Yoshikawa. Dynamics and stability in 
coordination of multiple robotic mechanisms. International Journal of 
Robotics Research, 8(2):44-61, 1989. 

[81] Ju. I. Neimark and N. A. Fufaev. Dynamics of Nonholonomic Systems, 
volume 33 of Translations of Mathematical Monographs. American Math¬ 
ematical Society, 1972. 

[82] V.-D. Nguyen. Constructing force-closure grasps. International Journal 
of Robotics Research, 7(3):3-16, 1988. 

[83] H. Nijmeijer and A. J. van der Schaft. Nonlinear Dynamical Control 
Systems. Springer-Verlag, 1990. 


445 


[84] T. Okada. Computer control of multijointed finger system for precise 
object-handling. IEEE Transactions on Systems, Man and Cybernetics, 
SMC-12(3) :289-299, 1982. 

[85] B. Paden. Kinematics and Control Robot Manipulators. PhD thesis, 
Department of Electrical Engineering and Computer Sciences, University 
of California, Berkeley, 1986. 

[86] B. Paden and S. S. Sastry. Optimal kinematic design of 6R manipulators. 
International Journal of Robotics Research, 7(2):43-61, 1988. 

[87] F. C. Park, J. E. Bobrow, and S. R. Ploen. A Lie group formulation of 
robot dynamics. UCI Mechanical Systems Technical Report, Department 
of Mechanical Engineering, University of California, Irvine, April 1993. 

[88] F. C. Park and A. P. Murray. Computational aspects of the product-of- 
exponentials formula for robot kinematics. IEEE Transactions on Auto¬ 
matic Control, 1994. (to appear). 

[89] L. A. Pars. A Treatise on Analytical Dynamics. Wiley, 1965. 

[90] R. P. Paul. Robot Manipulators: Mathematics, Programming and Control. 
MIT Press, 1981. 

[91] K. S. J. Pister. Hinged Polysilicon Structures with Integrated CMOS Thin 
Film Transistors. PhD thesis, Department of Electrical Engineering and 
Computer Sciences, University of California, Berkeley, 1992. 

[92] K. S. J. Pister, R. S. Fearing, and R. T. Howe. A planar air levitated 
space electrostatic actuator system. In IEEE Workshop on Micro Electro 
Mechanical Systems, pages 67-71, 1990. 

[93] K. S. J. Pister, M. W. Judy, S. R. Burgett, and R. S. Fearing. Micro- 
fabricated hinges. Sensors and Actuators A — Physical, 33(3):249-256, 

1992. 

[94] E. J. F. Primrose. On the input-output equation of the general 7R mech¬ 
anism. Mechanisms and Machine Theory, 21:509-510, 1986. 

[95] M. Raghavan. Manipulator kinematics. In R. W. Brockett, editor, 
Robotics: Proceedings of Symposia in Applied Mathematics, Volume fl, 
pages 21-48. American Mathematical Society, 1990. 

[96] M. Raghavan and B. Roth. Inverse kinematics of the general 6R manip¬ 
ulator and related linkages. Journal of Mechanical Design, 115:502-508, 

1993. 

[97] R. T. Rockafellar. Convex Analysis. Princeton University Press, 1970. 

[98] G. Rodriguez, A. Jain, and K. Kreutz-Delgado. A spatial operator algebra 
for manipulator modeling and control. International Journal of Robotics 
Research, 10(4):371-381, 1991. 

[99] R. M. Rosenberg. Analytical Dynamics of Discrete Systems. Plenum 
Press, New York, 1977. 

[100] B. Roth, J. Rastegar, and V. Scheinman. On the design of computer 
controlled manipulators. On the Theory and Practice of Robots and Ma¬ 
nipulators, Proceedings of the First CISM-IFToMM Symposium, pages 
93-113, 1973. 


446 


[101] J. K. Salisbury. Kinematic and Force Analysis of Articulated Hands. PhD 
thesis, Department of Mechanical Engineering, Stanford University, 1982. 

[102] S. S. Sastry and M. Bodson. Adaptive Control: Stability, Convergence, 
and Robustness. Prentice-Hall, 1989. 

[103] S. S. Sastry and R. Montgomery. The structure of optimal controls for 
a steering problem. In IFAC Symposium on Nonlinear Control Systems 
Design (NOLCOS), pages 385-390, 1992. 

[104] J-P. Serre. Lie Algebras and Lie groups. W. A. Benjamin, New York, 
1965. 

[105] T. Shamir and Y. Yomdin. Repeatability of redundant manipulators: 
Mathematical solution of the problem. IEEE Transactions on Automatic 
Control, 33(11)=1004-1009, 1988. 

[106] F. Skinner. Designing a multiple prehension manipulator. Journal of 
Mechanical Engineering, 97(9):30-37, 1975. 

[107] J. E. Slotine and W. Li. On the adaptive control of robot manipulators. 
International Journal of Robotics Research, 6:49-59, 1987. 

[108] M. Spivak. A Comprehensive Introduction to Differential Geometry, vol¬ 
ume I. Publish or Perish, Inc., Houston, second edition, 1979. 

[109] M. W. Spong, F. L. Lewis, and C. T. Abdallah, editors. Robot Control: 
Dynamics, Motion Planning and Analysis. IEEE Press, 1991. 

[110] M. W. Spong and M. Vidyasagar. Dynamics and Control of Robot Ma¬ 
nipulators. John Wiley, 1989. 

[111] D. Stewart. A platform with six degrees of freedom. Proceedings of the 
Institute of Mechanical Engineering, 180, part I(5):371-186, 1954. 1965- 
66 . 

[112] D. Tilbury, R. M. Murray, and S. S. Sastry. Trajectory generation for 
the N-trailer problem using Goursat normal form. In IEEE Control and 
Decision Conference, pages 971-977, 1993. 

[113] R. Tomovic and G. Boni. An adaptive artificial hand. IRE Transactions 
on Automatic Control, 7(3):3—10, 1962. 

[114] J. P. Trevelyan. Robots for Shearing Sheep: Shear Magic. Oxford Uni¬ 
versity Press, York, 1992. 

[115] V. S. Varadarajan. Lie Groups, Lie Algebras, and Their Representations. 
Springer-Verlag, 1984. 

[116] T. Venkataraman and T. Iberall, editors. Dextrous Robot Hands. 
Springer-Verlag, 1988. 

[117] A. M. Vershik and V. Ya. Gershkovich. Nonholonomic problems and 
the theory of distributions. Acta Applicandae Mathematicae, 12:181-209, 
1988. 

[118] M. Vidyasagar. Nonlinear Systems Analysis. Prentice-Hall, second edi¬ 
tion, 1993. 


447 


[119] G. Walsh and S. S. Sastry. On reorienting rigid linked bodies using inter¬ 
nal motions. In IEEE Control and Decision Conference, pages 1190-1195, 
1991. (to appear in IEEE Transactions on Robotics and Automation, 
1994). 

[120] J. T. Wen and D. S. Bayard. New class of control laws for robot ma¬ 
nipulators. Part 1: Non-adaptive case. International Journal of Control, 
47(5):1361-1385, 1988. 

[121] S. Wolfram. Mathematical A System for Doing Mathematics by Com¬ 
puter. Addison-Wesley, 1992. 

[122] T. Yoshikawa. Foundations of Robotics: Analysis and Control. MIT 
Press, 1990. 

[123] L. C. Young. Lectures on the Calculus of Variations and Optimal Control 
Theory. Chelsea, New York, second edition, 1980. 


448 


Index 


actions of Lie groups, 415-416 
actuator redundancy, 286 
actuator singularities, 135, 141 
actuators, types of, 155 
AdeptOne robot, 5, 83 
adjoint action, 415, 420, 421 
adjoint transformation, 55 

between body and spatial manipula¬ 
tor Jacobian, 117, 125 
between body and spatial velocities, 

55, 56 

for general Lie groups, 415 
for planar motions, 76 
properties of, 77 
of twists, 56, 59, 94 
of velocities, 59, 421 
of wrenches, 62, 63, 422 
admissible velocities, for parallel manipu¬ 
lators, 134 

angular velocity, see rotational velocity 
antipodal grasp, 232, 233 
asymptotic stability, 179, 180 
atan2, 32 

automobile, see kinematic car 
axis of a screw, 45 

choice of point on, 49 
axis of a twist, 47 
axis of a wrench, 65 

ball and socket joint, see spherical joint 
Ball, R. S., 19 
base frame, 84, 91 
biological motor control, 303, 307 
body angular velocity, 52 
body frame, 22, 23, 51 
body manipulator Jacobian, see manipu¬ 
lator Jacobian 
body velocity, 55, 419 

geometric interpretation, 55 
relationship with spatial velocity, 55, 

56, 61, 420 

transformation and addition of, 59 
body wrench, 63 

Campbell-Baker-Hausdorff formula, 381 


car with N trailers, 349 
Caratheodory’s theorem, 230, 299 
Cayley parameters, 73 
center of mass, 161 
chained form, 363, 364, 392 
conversion to, 369 

change of coordinates, see coordinate trans¬ 
formations 

Chasles’ theorem, 19, 49, 418 
Chen-Fliess series, 376, 378 
Chow’s theorem, 329, 341 
Christoffel symbols, 170, 246 
closed-chain manipulators, see parallel ma¬ 
nipulators 

coadjoint action, 416, 422 
coefficient of friction, 216, 218 
collinear re volute joints, 124 
commutator, 324 
complete workspace, 95 
completely nonholonomic, 320, 339 
computed torque, 190-192, 198, 204, 301 
condition number of a matrix, 128 
configuration of a rigid body, 22 
configuration space, 25, 35, 83, 165, 265 
conservation of angular momentum, 335 
constrained Lagrangian, 275 
constrained manipulators 

control of, 201-202, 209, 300, 428 
dynamics of, 200-201, 284 
planar example, 203 
constraints, 157, 266, 428 

forces of, 157, 200, 266-269, 428 
zz, see also internal forces 
holonomic, 157, 266, 318 
integrable, 267 

in multifingered grasps, 234-242, 253 
nonholonomic, 268, 274 
Pfaffian, 266-268 
contact coordinates, 249, 254 
contact forces, 215-218, 224, 238, 260, 277, 
280 

contact frame, 214, 246 
contact kinematics, 248-253 
planar, 262 

contact models, 214-218, 259 
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control 

of constrained manipulators, 201-202, 
209, 300 

of multifingered hands, 300-310 
of open-chain manipulators, 189-198 
problem description, 156 
of tendon-driven fingers, 298 
in workspace coordinates, 195-198 
controllability, 328-332 
controllability Lie algebra, 329 
controllability rank condition, 330 
convex hull, 225, 229 
convex set, 225 
coordinate chart, 243, 403 
coordinate frame, 20 
coordinate transformations 
on inertia matrix, 208 
invariance under, 78, 422-433 
on twists, 59, 77 

use in analyzing singularities, 125 
on velocities, 58, 421 
on wrenches, 62, 422 
coordinated lifting, 213, 263, 281 
coplanar revolute axes, 125, 150 
Coriolis and centrifugal forces, 165, 170 
Coriolis matrix, 171, 176, 279 
cotangent space, 326, 405 
Coulomb friction, 216 
coupling matrix, 295, 297 
covector, 326 
cross product 

2-dimensional, 232 
and Lie bracket, 175, 411 
matrix representation, 26 
preservation by rigid body transfor¬ 
mations, 21 
properties of, 26, 73 
curvature tensor, 245 
cylindrical joint, 81 

d’Alembert’s principle, 268, 271 
degree of nonholonomy, 340 
degrees of freedom, 84, 129, 303, 398 
of four-bar mechanism, 135 
loss of, 123, 127 
for parallel mechanisms, 133 
redundant, 285 

Denavit-Hartenberg parameters, 93, 110 
dextrous manipulation, 9, 213 
dextrous workspace, 95, 129 
dialytical elimination, 108 
diffeomorphism, 403 
direct method of Lyapunov, 181 
disk rolling on a plane, 272, 314, 336 
dispacement, rigid, 20 
distribution, 325 


drift-free control systems, 329 
dynamic finger repositioning, 382-388 
dynamics, 155 

constrained manipulators, 200-201, 
284 

multifingered hands, 276-285 
nonmanipulable grasps, 290-291 
open-chain manipulators, 168-178 
passivity property, 172, 209 
in presence of constraints, 265-276 
redundant manipulators, 286-290 
structural properties, 171, 197, 279, 
314 

using the product of exponentials for¬ 
mula, 175 

in workspace coordinates, 282 

eigenvalues of a rotation matrix, 30, 73 
elastic tendons, 296-299 
elbow manipulator, 147, 433 
forward kinematics, 89 
inverse kinematics, 104 
end-effector, 8, 83 
end-effector velocity 

using manipulator Jacobian, 115 
for parallel manipulators, 133 
end-effector wrench 

using manipulator Jacobian, 121-123, 
130 

for parallel manipulators, 134 
for redundant manipulators, 131 
at singular configuration, 124, 151 
Engel’s system, 373 
equilibrium point, 179 
equivalent axis representation, 31 
equivalent wrenches, 62 
Euler angles, 31, 150 
Euler’s equation, 166, 167, 208 
Euler’s theorem, 30 
Euler-Lagrange equations, 359 
exact one-form, 327 
exceptional surface, 230 
exponential coordinates 
on a Lie group, 414 
for rigid motion, 39-45 
zz, see also twists 
for rotation, 27-31 
exponential map 

as relative transformation, 42, 45, 49 
on general Lie group, 412 
for rigid body transformations, 41, 
413, 417 

for rotations, 28-29, 413 
surjectivity onto SE( 3), 42 
surjectivity onto SO( 3), 29 
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exponential of a matrix, see matrix expo¬ 
nential 

exponential stability, 180 
extension function, 294 

falling cat example, 352 
feedback linearization, 192 
feedforward control, 191, 309 
Fick angles, 32 
filtration, 340 

finger kinematics, 234-237, 253-254 
fingertip frame, 234 
firetruck example, 350 
first fundamental form, 244 
first-order controllable systems, 358 
fixed contact kinematics, 214 
flow of a vector field, 322, 406 
foliation, 326 

force control, see constrained manipula¬ 
tors, control of 
force-closure, 213 

for antipodal grasps, 232 
convexity conditions for, 226 
for grasping, 223 
number of contacts required, 230 
for tendon network, 299 
forward kinematics, 83-97 

for elbow manipulator, 89 
for parallel manipulators, 132 
product of exponentials formula, 85- 
91 

for redundant manipulators, 129 
for SCARA manipulator, 87, 92 
four-bar linkage, 135—138, 314 
frame, see coordinate frame, tool frame, 
base frame, etc. 
frame invariance, 78, 422 
free vector, see vector 
friction cone, 216, 218, 228, 229 
frictionless point contacts, 215, 220, 224 
Frobenius’ theorem, 326 
fundamental grasp constraints, see grasp 
constraints 

Gauss frame, 245 
Gauss map, 245 
Gauss-Bonnet theorem, 385 
general linear group, GL(n, R), 409, 410, 
412 

generalized coordinates, 158, 265, 274 
generalized forces, 158 
generalized inertia matrix, 162 
geometric parameters for a surface, 246 
geometric phase, 385 
global stability, 180 
grasp constraints 


fundamental grasp constraint, 237 
nonmanipulable case, 291 
redundant case, 289 
grasp map, 218-223 
grasping 

basic assumptions, 213, 214 
control, 300-310 
dynamics, 276-285 
effect of fingers, 234-242 
fixed contact kinematics, 214-223 
force relationships, 238 
kinematics and statics, 211-255 
versus parallel mechanisms, 281 
planar case, 222, 231, 232 
planning problem, 213, 229-234 
properties, see force-closure, manip- 
ulability 

representation of grasps, 220, 237 
rolling contact kinematics, 242-255 
similarity to parallel mechanisms, 134 
summary of properties, 239 
velocity constraints, 237 
group 

definition, 24 

of rigid body transformations, 37 
of rotations, 24 
growth vector, 341 
Gruebler’s formula, 133 

hand Jacobian, 236, 285 
harmonic oscillator, 185 
hazardous environments, 396 
helical joint, 81 
Helmholtz angles, 32 
hierarchical control, 302 
holonomic constraints, 157, 266, 318 
homogeneous coordinates, 19, 36-39, 417— 
419 

for points and vectors, 36, 417 
for rigid body transformations, 36, 
417 

homunculus diagram, 9 
hopping robot, 333, 341 
hybrid force control, see constrained ma¬ 
nipulators, control of 
hyperbolic metric on SE( 3), 426 

indirect method of Lyapunov, 184 
inelastic tendons, 294-296 
inertia matrix 

effect of coordinate transformation, 
208 

effective, in grasping, 279 
for open-chain manipulators, 168, 176 
for rigid bodies, 162, 208 
inertia tensor, 162, 166 
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infinite pitch screw, 48 
integrable constraints, 267, 318 
integrable distribution, 326 
integral manifolds, 326 
integrating factor, 319 
internal forces, 134, 223, 279, 301 
due to motion, 280, 290 
in grasping, 279-281 
regulation of, 301, 302 
in tendon network, 299 
internal motions, 130, 238, 285, 287 
intersecting joint axes, 126, 151 
invariant set, 188 

inverse elbow manipulator, 147, 433 
inverse kinematics, 97-114 

for elbow manipulator, 104 
general solutions, 108 
number of solutions, 98, 114 
for parallel manipulators, 133, 140 
for redundant manipulators, 130 
for SCARA manipulator, 106 
simple example, 97 
solving using subproblems, 98, 104 
for Stewart platform, 140 
involutive closure, 325 
involutive distribution, 325 
isotropic points, 150 

Jacobi identity, 325, 408 
Jacobian transpose, 121, 124 
Jacobian, manipulator, see manipulator 
Jacobian 
joint angle, 84 
joint space 

for open-chain manipulators, 83 
for parallel manipulators, 133 
joint space control, 156 

versus workspace control, 195, 198 
joint torques 

choice of, in grasping, 301 
and end-effector forces, 121, 289 
and tendon forces, 295 
joint twists, 87 

given Denavit-Hartenberg parameters, 
94 

joint types, 81 

Killing form, 427 
kinematic car, 318, 336, 343 
kinematic redundancy, 286 

zz, see also redundant manipulators 
kinematic singularities, 123-127, 150-151 
versus actuator singularities, 135, 141 
for four-bar mechanism, 137 
for open-chain manipulators, 124-127 
for parallel manipulators, 134 


kinematics, 81 
kinetic energy, 161 
Klein form, 426 

Lagrange multipliers, 157, 269-271 
formula for, 270 

relationship with contact forces, 280 
Lagrange’s equations, 158 

for constrained systems, 269, 275 
for mechanical systems, 156-167 
for open-chain manipulators, 169 
Lagrange-d’Alembert equations, 271, 272, 
275 

Lagrangian, 158 

for multifingered hand, 277 
for open-chain manipulators, 168 
Lasalle’s invariance principle, 188, 194 
leaf of a foliation, 326 
left invariant vector field, 409 
length scale, 424 
Lie algebra, 326, 407, 410 
Lie bracket, 175, 323-325, 407 
Lie bracket motion, 323 
Lie derivative, 322, 406 
Lie group, 408 
Lie product, 324, 344 
line contact, 260 
linearization, 184 
link frames, 93 
local controllability, 331 
local stability, 179, 180, 185 
locally positive definite functions, 182 
log function on a Lie group, 413 
loop equation, see structure equations 
lower pair joints, 81 
Lyapunov functions 
choosing, 183 
skewed energy, 186, 194 
Lyapunov stability, 178-189 
basic theorem, 182 
direct method, 181-184 
indirect method, 184-185 

magnitude of a twist, 48, 427 
magnitude of a wrench, 66 
manifold, 318 
manifold, definition of, 403 
manipulability measures, 127-129, 149, 151, 
429 

well-posed, 432 
manipulable grasp, 213, 237 
manipulator inertia matrix, 168 
manipulator Jacobian, 115-129 
body, 116 

geometric interpretation, 116 
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versus Jacobian of a mapping, 115, 
120 

and manipulability measures, 128 
for mapping forces, 121-123, 130 
for parallel manipulators, 133 
for redundant manipulators, 130 
relationship between body and spa¬ 
tial, 117 

for SCARA manipulator, 118, 122 
singularities, see kinematic singular¬ 
ities 

spatial, 116 

for Stanford manipulator, 119 
manipulator workspace, see workspace 
mass matrix, see manipulator inertia ma¬ 
trix 

Mathematica, 435 
matrix exponential, 19, 27, 40 
properties of, 74 

maximally independent contact regions, 
233 

medical robotics, 398 
metric tensor, 244 
microrobotics, 399 
minimally invasive surgery, 398 
Motoman, 282 
multifingered grasp, 237 
zz, see also grasping 
multifingered hand, 8 

limitations and advantages, 212 

Newton’s law, 157, 159, 166, 167 
Newton-Euler equations, 165-167, 314 
nilpotent Lie algebra, 344, 376 
nonholonomic constraints, 268, 274-276, 
318 

classification, 340 

versus holonomic constraints, 274 

integrating, 319 

zz, see also Pfaffian constraints 
nonholonomic motion planning, 319, 331 
nonmanipulable grasps, 239, 290 
normal vector, 244 
normalized Gauss frame, 245 
numbering conventions for a robot, 83 

lj limit set, 188 
one-forms, 326, 408 
open loop control, 190 
open-chain manipulators, 82 
optimal manipulator design, 432 
optimal steering, 371 
orthogonal coordinate chart, 244 
orthogonal matrices, see rotation matri¬ 
ces 


Paden-Kahan subproblems, 99-103, 147— 
148 

solving inverse kinematics using, 104 
palm frame, 215 
parallel manipulators, 132-142 
inverse kinematics, 133, 140 
kinematic singularities, 134 
zz, see also four bar linkage, Stewart 
platform 

passivity, 172, 187, 209 
PD control, 193-195 
perspective transformations, 37 
Pfaffian constraints, 266-268 

converting to control system, 320, 
327 

integrability conditions, 328 
Philip Hall basis, 344 
pitch of a screw, 45 
pitch of a twist, 47, 427 
pitch of a wrench, 65 
planar grasping, 222, 231, 232, 262 
planar joint, 82 

planar rigid body transformations, 76 
planar rotational motion, 75 
planar Stewart platform, 141 
plane contact, 260 
Poinsot’s theorem, 19, 64, 65 
point contact with friction, 217 
points 

rigid transformation of, 35, 36, 417 
rotational transformation of, 25 
versus vectors, 21, 36, 322 
position control, 189-198 
positive definite functions, 182 
positive span, 225, 230 
positively dependent, 225 
potential energy for an open-chain manip¬ 
ulator, 169 
prehensile grasp, 260 
prismatic joint, 40, 81, 84 

twist associated with, 48, 87 
product of exponentials formula, 82, 85— 
91 

basic formula, 87 
choice of base frame, 91 
versus Denavit-Hartenberg parame¬ 
ters, 93 

dynamics using, 175, 207 
independence of order of joint mo¬ 
tions, 146 

independence on order of joint mo¬ 
tions, 86 

manipulator Jacobian using, 116 
projection maps, 75 
prosthetic hands, 10 
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pseudo-inverse for resolving redundancy, 
130 

pull back map, 407, 408 
PUMA manipulator, 4, 6 

zz, see also elbow manipulator 
pure quaternion, 74 
pure rolling, 249, 252, 338 
push forward map, 407 

quaternions, 33-34, 74 

rank of structure equations, 134 
rate of convergence, 181, 184 
reachable set, 318, 320 
reachable workspace, 95 
reciprocal product, 66 
reciprocal screws, 66-69 
definition, 66 
systems of, 69, 78 

use in analyzing mechanisms, 67, 69, 
126 

redundant manipulators, 122 
dynamics, 286-290 
in grasping, 238 

kinematic versus actuator redundancy, 
286 

kinematics, 129-132 
reference configuration, 87 
choice of, 91 
regular distribution, 325 
regular filtration, 340 
relative curvature form, 250 
relative growth vector, 341 
relative motion, representation using the 
exponential map, 42 
re volute joint, 81, 84 

twist associated with, 48, 87 
right-handed coordinate frame, 22 
rigid bodies, 20 

dynamics, 165-167 
inertial properties, 160-163 
kinetic energy, 161 
rigid body motion, 34-50 
definition of, 20 

representation using SE( 3), 35, 416 
representation using body-fixed frame, 
22 

rigid body transformations, 20-22 

actions on points and vectors, 21, 
35-37, 417 
composition rule, 37 
formal definition, 21 
group properties, 37 
homogeneous representation, 36 
planar, 76 

rigid body velocity, 53-61, 418-420 


rigid displacement, 20 
rigid transformations, see rigid body trans¬ 
formations 

robot, origin of word, 1 
robustness of control laws, 190 
Rodrigues’ formula, 28, 76 
roll, pitch, yaw angles, 32 
rolling contact kinematics, 242-255 
rolling penny, see disk rolling on a plane 
rotation about a line, 38, 87, 99 
as a screw motion, 49 
twist coordinates, 43 
rotation about two axes, 100 
rotation group, 24 
rotation matrices, 23 

actions on points and vectors, 25 
eigenvalues of, 30 
properties of, 23-26, 73 
rotation to a given distance, 102 
rotational motion, 22-34 
composition rule, 25 
equivalent axis representation, 31 
Euler angle representation, 31 
exponential coordinates, 27-31 
about a fixed axis, 27, 29 
parameterization singularities, 31, 32 
planar, 75 

quaternion representation, 33 
representation using rotation matri¬ 
ces, 23 

rotational velocity, 51-53 

body versus spatial, 52 

Salisbury Hand, 11, 12 
SCARA manipulator, 6, 83 
dynamics, 177 
forward kinematics, 87, 92 
grasp using, 240, 291 
inverse kinematics, 106 
manipulator Jacobian, 118, 120, 122 
screw motions, 19, 45, 46 

instantaneous velocity of, 57 
screw system, 68 
screw theory 

advantages of, 20 
origins of, 19 
screws, 45-50 

associated with wrenches, 64 
Chasles’ theorem, 49 
geometric attributes of, 45-46 
infinite pitch, 48 

rigid body transformations associated 
with, 46 

twists associated with, 48 
SE( 3), 35, 409 

bi-invariant quadratic forms, 425 
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bi-invariant volume forms, 431 
hyperbolic metric, 426 
invariant metrics, 423 
lack of bi-invariant metric, 427 
metric properties, 422 
se(3), 40, 411 

second fundamental form, 245 
second-order controllable systems, 361 
self-motion manifold, 130 
separating hyperplane, 226 
setpoint stabilization, 193 
singular configurations, 123, 151 
for parallel manipulators, 134 
singular values of a matrix, 128, 148 
singularities, see kinematic singularities 
skew-symmetric matrices, 27 
properties of, 26, 28, 73 
for representing cross product, 26 
slider-crank mechanism, 151, 203, 314 
sliding, 249, 268 

small-time locally controllable, see locally 
controllable 
50(3), 24, 409 

zz, see also rotation matrices 
so(3), 28, 411 

zz, see also skew-symmetric matri¬ 
ces 

soft-finger contact, 217 
space robots, 334, 342, 351, 396 
spatial angular velocity, 52 
spatial frame, 51 

spatial manipulator Jacobian, see manip¬ 
ulator Jacobian 
spatial operator algebra, 207 
spatial velocity, 54, 419 
addition of, 58, 422 
geometric interpretation, 54 
relationship with body velocity, 55, 
56, 61, 420 

transformation of, 58, 421 
spatial wrench, 63 
special Euclidean group, see SE( 3) 
special orthogonal group, see 50(3) 
sphere rolling on a plane, 252, 338, 343 
sphere rolling on a sphere, 349 
spherical joint, 81, 138 
spherical wrist, 125 

effect on workspace, 96 
versus spherical joint, 139 
spring mass system, 185, 187, 189 
stability by linearization, 184 
stability definitions, 179-181 
stable, 179 

Stanford manipulator, 2, 4, 147 
manipulator Jacobian, 119 
Stanford/JPL hand, see Salisbury Hand 


Steinitz’s theorem, 230, 299 
Stewart platform, 138-142, 153 
strictly internal forces, 223 
structurally dependent forces, 122, 239 
structure equations, 132-134 

for four-bar mechanism, 136 
for Stewart platform, 140 
supporting hyperplane, 226 
surface models, 243 

tangent space, 243, 404 
teleoperation, 395 
tendon kinematics, 293-300 
tool frame, 84 
torsion form, 246 

trajectory generation, using manipulator 
Jacobian, 117 

trajectory tracking, see position control 
translational motion, 34, 48 
transpose of Jacobian, see Jacobian trans¬ 
pose 

twists, 19, 417 

definition of, 41 

geometric attributes, 45-50, 427 
Lie bracket between, 175 
parameterizing manipulators via, OI¬ 
OS 

reciprocal to a wrench, 66 
for revolute and prismatic joints, 87 
screw coordinates, 47 
screw motions corresponding to, 48 
transformation of, 59, 77 
twist coordinates, 41 
two-link planar manipulator 
constrained, 315 
dynamics, 164 
inverse kinematics, 97 
moving in a slot, 203 

U-joint, 153 

uncertainty configuration, 137 
underwater robots, 397 
uniform stability, 179, 185 
unit quaternions, 34, 74 
unit twist, 49 

Utah/MIT hand, 10, 12, 212 

variable geometry truss, 152 
vector field, 322, 406 
vectors, 21 

versus points, 21, 36 
rigid transformation of, 21, 37, 417 
rotational transformation of, 25 
velocity 

end-effector, 115 

rigid body, see rigid body velocity 
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rotational, see rotational velocity 
of a screw motion, 57 
velocity of a point 

attached to end-effector, 117 
for rotational motion, 52 
virtual displacement, 271 
virtual reality, 396 
virtual work, 271 
viscous friction, 170 
volume forms on SE(3), 430 

work, between twist and wrench, 61 
workspace control, 156, 195—198, 209 

versus joint space control, 195, 198 
workspace dynamics, 197, 282 
workspace of a manipulator, 95-97, 432 
dextrous, 95, 129 
maximal, 433 

wrench basis for a contact, 217, 235 
wrenches, 19, 61-66, 420 
addition of, 63 

body and spatial representations, 63 
reciprocal to a twist, 66 
screw coordinates of, 64 
transformation of, 62, 422 

zero pitch screw, 48, 66 
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